Avr/Plotter
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Revamped Delay timeing

Browse source 
Added Backlash Compensation
This commit is contained in:
IMback 2017-09-25 19:16:57 +02:00
parent cc1bdaa82f
commit 303f32ad6d
2 changed files with 67 additions and 60 deletions
Download patch file Download diff file Expand all files Collapse all files

112
plotter.cpp
View file

@ -2,6 +2,8 @@
const int SCALEX=2;
const int SCALEY=3;
const int BACKLASHX=120;
const int BACKLASHY=120;
template <typename T> int sgn(T val)
{
@ -58,36 +60,49 @@ void Plotter::pu()
}
}
void Plotter::moveto(Point *pt)
{
moveto((*pt).x,(*pt).y);
}
uint16_t Plotter::diamondAngle(int16_t y, int16_t x) //0 - 40000
static 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));
}
void Plotter::moveto(const uint16_t nx, const uint16_t ny)
static uint16_t calculateDelayTime(uint16_t currentDiamondAngle, uint16_t prevDiamondAngle, uint16_t prevSteps)
{
int16_t deltaX = nx - currentPos.x; //117 units per mm
int16_t deltaY = ny - currentPos.y; //121 units per mm
/*_serial->write("deltaX: ");
_serial->write(deltaX);
_serial->write(" deltaY: ");
_serial->write(deltaY);*/
uint16_t steps = 0; //one step is max 1.28205mm min 0.8547mm
if(deltaX != 0 || deltaY != 0)
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 ((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
@ -100,48 +115,39 @@ void Plotter::moveto(const uint16_t nx, const uint16_t ny)
StepSizeY = 100*sgn(deltaY);
if(StepSizeY != 0) steps = deltaY/StepSizeY;
}
uint16_t currentDiamondAngle = diamondAngle(deltaX,deltaY);
uint16_t delayTime = abs(currentDiamondAngle - prevDiamondAngle);
if ( delayTime > 20000) delayTime = delayTime - 20000;
if( deltaX > 50 || deltaY > 50 )
{
if( prevSteps > 4 || delayTime > 10000 ) for (uint16_t i = 0; i < delayTime; i++) _delay_us(50);
else for(uint16_t j = 0; j < prevSteps+1; j++) for(uint16_t i = 0; i < delayTime; i++) _delay_us(5);
}
else if( deltaX > 20 || deltaY > 20 ) for (uint16_t i = 0; i < delayTime/4; i++) _delay_us(1);
else _delay_us(100);
/*_serial->write(" currentDiamondAngle: ");
_serial->write(currentDiamondAngle);
_serial->write(" delayTime: ");
_serial->write(delayTime);*/
if( deltaX > 20 || deltaY > 20 )
{
prevDiamondAngle = currentDiamondAngle;
prevSteps = steps;
}
//_serial->putChar('\n');
//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);
}
}
else delay_us(100);
_pwm.setDutyA(65535-nx*SCALEX);
_pwm.setDutyB(65535-ny*SCALEY);
_pwm.setDutyA(65535-(deltaX+currentPos.x)*SCALEX);
_pwm.setDutyB(65535-(deltaY+currentPos.Y)*SCALEY);
currentPos.x = nx;
currentPos.y = ny;
if( deltaX > 50 || deltaY > 50 )
{
//set prev
prevDiamondAngle = currentDiamondAngle;
prevSteps = steps;
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
moveRelatvie(deltaX, deltaY);
}

15
plotter.h
View file

@ -18,17 +18,17 @@ private:
Pwm16b _pwm;
Point currentPos = {0, 0};
uint16_t prevDiamondAngle = 60000;
uint16_t prevSteps = 10;
uint16_t _prevDiamondAngle = 60000;
uint16_t _prevSteps = 10;
volatile unsigned char *_penPort;
char _penPin;
Serial* _serial;
uint16_t diamondAngle(int16_t y, int16_t x);
static uint16_t diamondAngle(int16_t y, int16_t x);
static uint16_t calculateDelayTime(uint16_t currentDiamondAngle, uint16_t prevDiamondAngle, uint16_t prevSteps);
public:
Plotter(volatile unsigned char *penPort, const char penPin, Serial* serial);
@ -38,7 +38,8 @@ public:
void pu();
void moveto(Point *pt);
void moveto(uint16_t nx,uint16_t ny);
void moveRelative(int32_t deltaX, int32_t deltaY);
void moveRelative(Point *pt);
Point getCurrentPos();
};