Revamped Delay timeing

Added Backlash Compensation

plotter.cpp

@@ -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)
+static uint16_t Plotter::diamondAngle(int16_t y, int16_t x) //0 - 40000
-{
-	moveto((*pt).x,(*pt).y);
-}
-
-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
+    uint16_t delayTime = abs(currentDiamondAngle - prevDiamondAngle);
-    int16_t deltaY = ny - currentPos.y; //121 units per mm
+    if ( delayTime > 20000) delayTime = delayTime - 20000;
-    
+    if( prevSteps <= 4 && delayTime < 10000 ) delayTime = delayTime/10;
-    /*_serial->write("deltaX: ");
+    return delayTime;
-    _serial->write(deltaX);
+}
-    _serial->write(" deltaY: ");
+
-    _serial->write(deltaY);*/
+void Plotter::moveRelative(Point *pt)
-    
+{
-    uint16_t steps = 0; //one step is max 1.28205mm min 0.8547mm
+    moveRelative((*pt).x,(*pt).y);
-    
+}
-    if(deltaX != 0 || deltaY != 0)
+
+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;
         }
-        
+    
-        
+        //interpolate
-        
-            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');
-            
         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.setDutyA(65535-(deltaX+currentPos.x)*SCALEX);
-    _pwm.setDutyB(65535-ny*SCALEY);
+    _pwm.setDutyB(65535-(deltaY+currentPos.Y)*SCALEY);
     
-    currentPos.x = nx;
+    if( deltaX > 50 ||  deltaY > 50 )
-    currentPos.y = ny;
+    {
+        //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);
 }
 

plotter.h

@@ -18,17 +18,17 @@ private:
     Pwm16b _pwm;
     Point currentPos = {0, 0};
     
-    uint16_t prevDiamondAngle = 60000;
+    uint16_t _prevDiamondAngle = 60000;
-    uint16_t prevSteps = 10;
+    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();
 };