155 lines
4.2 KiB
C++
155 lines
4.2 KiB
C++
#include "plotter.h"
|
|
|
|
const int SCALEX=2;
|
|
const int SCALEY=3;
|
|
const int BACKLASHX=120;
|
|
const int BACKLASHY=120;
|
|
|
|
template <typename T> int sgn(T val)
|
|
{
|
|
int value = (T(0) < val) - (val < T(0));
|
|
return (value == 0) ? 1 : value;
|
|
}
|
|
|
|
Plotter::Plotter(volatile unsigned char *penPort, const char penPin, Serial* serial): _pwm( &TCCR1A, &TCCR1B, &OCR1A, &OCR1B, &ICR1, 0b00000001)
|
|
{
|
|
_serial = serial;
|
|
_penPort=penPort;
|
|
_penPin=penPin;
|
|
basicposition();
|
|
}
|
|
|
|
Point Plotter::getCurrentPos()
|
|
{
|
|
return currentPos;
|
|
}
|
|
|
|
|
|
void Plotter::demo()
|
|
{
|
|
moveto(65535,0);
|
|
moveto(65535,42129);
|
|
moveto(0,42129);
|
|
moveto(0,0);
|
|
pd();
|
|
pu();
|
|
}
|
|
|
|
void Plotter::basicposition()
|
|
{
|
|
moveto(0, 0);
|
|
}
|
|
|
|
void Plotter::pd()
|
|
{
|
|
if(!readPin(_penPort, _penPin))
|
|
{
|
|
_delay_us(200000);
|
|
writePin(_penPort, _penPin, true);
|
|
_delay_us(500);
|
|
}
|
|
}
|
|
|
|
void Plotter::pu()
|
|
{
|
|
if(readPin(_penPort, _penPin))
|
|
{
|
|
_delay_us(100000);
|
|
writePin(_penPort, _penPin, false);
|
|
_delay_us(500);
|
|
}
|
|
}
|
|
|
|
uint16_t Plotter::diamondAngle(int16_t y, int16_t x) //0 - 40000
|
|
{
|
|
const int32_t scalar = 10000;
|
|
if (y >= 0) return (x >= 0 ? ((int32_t)y*scalar)/(x+y) : 1*scalar-((int32_t)x*scalar)/(-x+y));
|
|
else return (x < 0 ? 2*scalar-((int32_t)y*scalar)/(-x-y) : 3*scalar+((int32_t)x*scalar)/(x-y));
|
|
}
|
|
|
|
uint16_t Plotter::calculateDelayTime(uint16_t currentDiamondAngle, uint16_t prevDiamondAngle, uint16_t prevSteps)
|
|
{
|
|
uint16_t delayTime = abs(currentDiamondAngle - prevDiamondAngle);
|
|
if ( delayTime > 20000) delayTime = delayTime - 20000;
|
|
if( prevSteps <= 4 && delayTime < 10000 ) delayTime = delayTime/10;
|
|
return delayTime;
|
|
}
|
|
|
|
void Plotter::moveRelative(Point *pt)
|
|
{
|
|
moveRelative((*pt).x,(*pt).y);
|
|
}
|
|
|
|
void Plotter::moveRelative(int32_t deltaX, int32_t deltaY)
|
|
{
|
|
if( deltaX > 50 || deltaY > 50 )
|
|
{
|
|
//compensate backlash
|
|
if((_prevDiamondAngle < 10000 || _prevDiamondAngle > 30000 ) && deltaX < 0) deltaX = deltaX - BACKLASHX;
|
|
else if ((_prevDiamondAngle > 10000 && _prevDiamondAngle < 30000 ) && deltaX > 0) deltaX = deltaX + BACKLASHX;
|
|
|
|
if((_prevDiamondAngle < 20000) && deltaY < 0) deltaY = deltaY - BACKLASHY;
|
|
else if ((_prevDiamondAngle > 20000) && deltaY > 0) deltaY = deltaY + BACKLASHY;
|
|
|
|
//delay
|
|
uint16_t currentDiamondAngle = diamondAngle(deltaX,deltaY);
|
|
uint16_t delayTime = calculateDelayTime(currentDiamondAngle, _prevDiamondAngle, _prevSteps);
|
|
if ( delayTime > 20000) delayTime = delayTime - 20000;
|
|
for (uint16_t i = 0; i < delayTime; i++) _delay_us(50);
|
|
|
|
//calculate Interporlation
|
|
uint16_t steps = 0; //one step is max 1.28205mm min 0.8547mm
|
|
|
|
int32_t StepSizeX=0;
|
|
int32_t StepSizeY=0;
|
|
|
|
if( abs(deltaX) > abs(deltaY))
|
|
{
|
|
StepSizeX = 100*sgn(deltaX);// -100
|
|
if(deltaY != 0) StepSizeY = ((int32_t)abs(deltaY)*100)/abs(deltaX)*sgn(deltaY); // 0
|
|
if(StepSizeX != 0) steps = deltaX/StepSizeX;
|
|
}
|
|
else
|
|
{
|
|
if(deltaX != 0) StepSizeX = ((int32_t)abs(deltaX)*100)/abs(deltaY)*sgn(deltaX);
|
|
StepSizeY = 100*sgn(deltaY);
|
|
if(StepSizeY != 0) steps = deltaY/StepSizeY;
|
|
}
|
|
|
|
//interpolate
|
|
for( uint16_t i = 0; i < steps; i++)
|
|
{
|
|
_pwm.setDutyA(65535-(currentPos.x+i*StepSizeX)*SCALEX);
|
|
_pwm.setDutyB(65535-(currentPos.y+i*StepSizeY)*SCALEY);
|
|
_delay_us(2000);
|
|
}
|
|
//set prev
|
|
_prevDiamondAngle = currentDiamondAngle;
|
|
_prevSteps = steps;
|
|
}
|
|
else _delay_us(100);
|
|
|
|
_pwm.setDutyA(65535-(deltaX+currentPos.x)*SCALEX);
|
|
_pwm.setDutyB(65535-(deltaY+currentPos.y)*SCALEY);
|
|
|
|
if( deltaX > 50 || deltaY > 50 )
|
|
{
|
|
//set current position
|
|
currentPos.x = deltaX+currentPos.x;
|
|
currentPos.y = deltaY+currentPos.y;
|
|
}
|
|
}
|
|
|
|
void Plotter::moveto(Point *pt)
|
|
{
|
|
moveto((*pt).x,(*pt).y);
|
|
}
|
|
|
|
void Plotter::moveto(const uint16_t nx, const uint16_t ny)
|
|
{
|
|
int32_t deltaX = nx - currentPos.x; //117 units per mm
|
|
int32_t deltaY = ny - currentPos.y; //121 units per mm
|
|
moveRelative(deltaX, deltaY);
|
|
}
|
|
|