Motion Control Updates

main.cpp

@@ -10,14 +10,18 @@ int main()
 {
     DDRB =  0xFF; 
     
-    sei();
+    
     Serial serial;
+    sei();
     
+    /*serial.write("UVOS plotter interface\n");
+    serial.write(1250);
+    serial.putChar('\n');*/
     
-    Plotter plotter(&PORTB, PB4);
+    Plotter plotter(&PORTB, PB4, &serial);
     HpglParser parser(&plotter);
     
-    //serial.putString("UVOS plotter interface\n");
+    
     while(true)
     {
         if(serial.dataIsWaiting())
@@ -26,11 +30,11 @@ int main()
             int parseRetrun = parser.add(ch);
             if(parseRetrun == 1) 
             {
-                serial.putString("ok\n");
+                serial.write("ok\n");
             }
             else if(parseRetrun == 2) 
             {
-                serial.putString("overrun!, ok\n");
+                serial.write("overrun!, ok\n");
             }
         }
     }

plotter.cpp

@@ -3,8 +3,15 @@
 const int SCALEX=2;
 const int SCALEY=3;
 
-Plotter::Plotter(volatile unsigned char *penPort,  const char penPin): _pwm( &TCCR1A, &TCCR1B, &OCR1A, &OCR1B, &ICR1, 0b00000001)
+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();
@@ -35,6 +42,7 @@ void Plotter::pd()
 {
         if(!readPin(_penPort, _penPin))
         {
+            _delay_us(200000);
             writePin(_penPort, _penPin, true);
             _delay_us(500);
         }
@@ -44,6 +52,7 @@ void Plotter::pu()
 {
     if(readPin(_penPort, _penPin))
     {
+        _delay_us(100000);
 	writePin(_penPort, _penPin, false);
 	_delay_us(500);
     }
@@ -54,46 +63,83 @@ void Plotter::moveto(Point *pt)
 	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)
 {
-    int deltaX = nx - currentPos.x; //-5100
-    int deltaY = ny - currentPos.y; //0
+    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)
+    {
         
-    int steps = 0;
         int32_t StepSizeX=0;
         int32_t StepSizeY=0;
         
         if( abs(deltaX) > abs(deltaY))
         {
-        if(deltaX != 0) StepSizeX = 100*(deltaX/abs(deltaX));// -100
-        if(deltaY != 0) StepSizeY = ((int32_t)abs(deltaY)*100)/abs((int32_t)deltaX)*(deltaY/abs(deltaY));// 0
-        steps = deltaX/StepSizeX;
+            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
         {
-        StepSizeX = (((int32_t)abs(deltaX)*100)/abs((int32_t)deltaY))*(deltaX/abs(deltaX));
-        StepSizeY = 100*(deltaY/abs(deltaY));
-        steps = deltaY/StepSizeY;
+            if(deltaX != 0) StepSizeX = ((int32_t)abs(deltaX)*100)/abs(deltaY)*sgn(deltaX);
+            StepSizeY = 100*sgn(deltaY);
+            if(StepSizeY != 0) steps = deltaY/StepSizeY;
         }
         
-    for( int i = 0; i < steps; i++)
+        
+        
+            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(5000);
+            _delay_us(2000);
         }
     
+    }
 
     _pwm.setDutyA(65535-nx*SCALEX);
     _pwm.setDutyB(65535-ny*SCALEY);
-    _delay_us(10000);
-    
-    if(steps < 50) for(int i = 0; i < steps; i++)_delay_us(11000);
-    else if(steps < 5) _delay_us(10000*5);
-    else _delay_us(200000);
-    
-    //_delay_us(500000);
-    //else _delay_us(10000);
     
     currentPos.x = nx;
     currentPos.y = ny;

plotter.h

@@ -10,6 +10,7 @@
 #include "writepin.h"
 #include <stdio.h>
 #include <stdlib.h> 
+#include "serial.h"
 
 class Plotter
 {
@@ -17,11 +18,20 @@ private:
     Pwm16b _pwm;
     Point currentPos = {0, 0};
     
+    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);
+    
 public:
-    Plotter(volatile unsigned char *penPort,  const char penPin);
+    Plotter(volatile unsigned char *penPort,  const char penPin, Serial* serial);
     void demo();
     void basicposition();
     void pd();

serial.cpp

@@ -38,7 +38,7 @@ void Serial::putChar(const char c)
     UDR0 = c;
 }
 
-void Serial::putString(const char* in, const unsigned int length)
+void Serial::write(const char* in, const unsigned int length)
 {
     for(unsigned int i = 0; i < length && in[i] != '\0'; i++)
     {
@@ -46,7 +46,7 @@ void Serial::putString(const char* in, const unsigned int length)
     }
 }
 
-void Serial::putString(const char in[])
+void Serial::write(const char in[])
 {
     for(unsigned int i = 0; i < strlen(in); i++)
     {
@@ -54,6 +54,35 @@ void Serial::putString(const char in[])
     }
 }
 
+
+void Serial::write(int32_t in)
+{
+    if(in == 0)
+    {
+        putChar('0');
+    }
+    else
+    {
+        bool flag = false;
+        char str[64] = { 0 }; 
+        int16_t i = 62;
+        if (in < 0) 
+        {
+            flag = true;
+            in = abs(in);
+        }
+
+        while (in != 0 && i > 0) 
+        { 
+            str[i--] = (in % 10) + '0';
+            in /= 10;
+        }
+
+        if (flag) str[i--] = '-';
+        write(str + i + 1, 64-(i+1));
+    }
+}
+
 bool Serial::dataIsWaiting()
 {
     return (interruptIndex > _rxIndex);

serial.h

@@ -8,6 +8,7 @@
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <string.h>
+#include <stdlib.h>
 
 const bool serialFlowControl = false;
 
@@ -20,8 +21,9 @@ private:
 public:
     Serial();
     void putChar(const char c);
-    void putString(const char* in, const unsigned int length);
-    void putString(const char in[]);
+    void write(const char* in, const unsigned int length);
+    void write(const char in[]);
+    void write(const int32_t in);
     bool dataIsWaiting();
     char getChar();
     unsigned int getString(char* buffer, const int bufferLength);