From 303f32ad6df2751cd08a86ef068deed84e843014 Mon Sep 17 00:00:00 2001 From: IMback Date: Mon, 25 Sep 2017 19:16:57 +0200 Subject: [PATCH] Revamped Delay timeing Added Backlash Compensation --- plotter.cpp | 112 +++++++++++++++++++++++++++------------------------- plotter.h | 15 +++---- 2 files changed, 67 insertions(+), 60 deletions(-) diff --git a/plotter.cpp b/plotter.cpp index 598ae17..40043ad 100644 --- a/plotter.cpp +++ b/plotter.cpp @@ -2,6 +2,8 @@ const int SCALEX=2; const int SCALEY=3; +const int BACKLASHX=120; +const int BACKLASHY=120; template 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); } diff --git a/plotter.h b/plotter.h index 00fbed9..4e0f157 100644 --- a/plotter.h +++ b/plotter.h @@ -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(); };