diff --git a/hpglparser.cpp b/hpglparser.cpp index 88a5e19..4041037 100644 --- a/hpglparser.cpp +++ b/hpglparser.cpp @@ -68,6 +68,18 @@ void HpglParser::findCommand() cmdMode = SKIP_M; bufferIndex = -1; } + else if(buffer[bufferIndex-1] == 'H') + { + _plotter->setHighPrecision(true); + cmdMode = SKIP_M; + bufferIndex = -1; + } + else if(buffer[bufferIndex-1] == 'L') + { + _plotter->setHighPrecision(false); + cmdMode = SKIP_M; + bufferIndex = -1; + } break; } case 'U': @@ -110,6 +122,17 @@ void HpglParser::findCommand() else cmdMode = ERROR_M; break; } + case 'E': + { + if(buffer[bufferIndex-1] == 'D') + { + _plotter->demo(); + cmdMode = SKIP_M; + bufferIndex = -1; + } + else cmdMode = ERROR_M; + break; + } } } diff --git a/plotter.cpp b/plotter.cpp index 90521f9..48fabb9 100644 --- a/plotter.cpp +++ b/plotter.cpp @@ -2,8 +2,8 @@ const int SCALEX=2; const int SCALEY=3; -const int BACKLASHX=120; -const int BACKLASHY=120; +const int BACKLASHX=20; +const int BACKLASHY=20; template int sgn(T val) { @@ -27,10 +27,16 @@ Point Plotter::getCurrentPos() void Plotter::demo() { - moveto(65535,0); - moveto(65535,42129); - moveto(0,42129); + moveto(0,0); + _delay_us(200000); + moveto(32760,0); + _delay_us(200000); + moveto(32760,21840); + _delay_us(200000); + moveto(0,21840); + _delay_us(200000); moveto(0,0); + _delay_us(200000); pd(); pu(); } @@ -71,32 +77,49 @@ uint16_t Plotter::calculateDelayTime(uint16_t currentDiamondAngle, uint16_t prev { uint16_t delayTime = abs(currentDiamondAngle - prevDiamondAngle); if ( delayTime > 20000) delayTime = delayTime - 20000; - if( prevSteps <= 4 && delayTime < 10000 ) delayTime = delayTime/10; + if( prevSteps <= 4 && delayTime < 3000 ) delayTime = delayTime/5; 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 ) + if( abs(deltaX) > 50 || abs(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; + if( 0 > ((deltaX+currentPos.x)*SCALEX)) + { + moveto(0,deltaY+currentPos.y); + return; + } + else if( ~(uint16_t)0 <= (deltaX+currentPos.x)*SCALEX) + { + moveto(32760,deltaY+currentPos.y); + return; + } + if( 0 > (deltaY+currentPos.y)*SCALEY) + { + moveto(deltaX+currentPos.x,0); + return; + } + else if( ~(uint16_t)0 <= (deltaY+currentPos.y)*SCALEY) + { + moveto(deltaX+currentPos.x,21840); + return; + } + //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); + /*//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;*/ + //calculate Interporlation uint16_t steps = 0; //one step is max 1.28205mm min 0.8547mm @@ -106,33 +129,39 @@ void Plotter::moveRelative(int32_t deltaX, int32_t deltaY) 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; + StepSizeY = (deltaY*100)/abs(deltaX); // 0 + steps = deltaX/StepSizeX; } else { - if(deltaX != 0) StepSizeX = ((int32_t)abs(deltaX)*100)/abs(deltaY)*sgn(deltaX); + StepSizeX = (deltaX*100)/abs(deltaY); StepSizeY = 100*sgn(deltaY); - if(StepSizeY != 0) steps = deltaY/StepSizeY; + 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); + _pwm.setDutyA(~((uint16_t)(currentPos.x+i*StepSizeX)*SCALEX)); + _pwm.setDutyB(~((uint16_t)(currentPos.y+i*StepSizeY)*SCALEY)); + _delay_us(2500); } //set prev _prevDiamondAngle = currentDiamondAngle; _prevSteps = steps; } - else _delay_us(100); + else + { + _delay_us(100); + } - _pwm.setDutyA(65535-(deltaX+currentPos.x)*SCALEX); - _pwm.setDutyB(65535-(deltaY+currentPos.y)*SCALEY); + _pwm.setDutyA(~((uint16_t)(deltaX+currentPos.x)*SCALEX)); + _pwm.setDutyB(~((uint16_t)(deltaY+currentPos.y)*SCALEY)); - if( deltaX > 50 || deltaY > 50 ) + _delay_us(3000); + if(highPrecision) _delay_us(80000); + + if( abs(deltaX) > 50 || abs(deltaY) > 50 || highPrecision ) { //set current position currentPos.x = deltaX+currentPos.x; @@ -140,6 +169,11 @@ void Plotter::moveRelative(int32_t deltaX, int32_t deltaY) } } +void Plotter::setHighPrecision(bool in) +{ + highPrecision = in; +} + void Plotter::moveto(Point *pt) { moveto((*pt).x,(*pt).y); @@ -147,8 +181,8 @@ void Plotter::moveto(Point *pt) 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 + int32_t deltaX = (int32_t)nx - (int32_t)currentPos.x; //117 units per mm + int32_t deltaY = (int32_t)ny - (int32_t)currentPos.y; //121 units per mm moveRelative(deltaX, deltaY); } diff --git a/plotter.h b/plotter.h index 55e8676..0cd4be1 100644 --- a/plotter.h +++ b/plotter.h @@ -9,7 +9,7 @@ #include "pwm.h" #include "writepin.h" #include -#include +#include #include "serial.h" class Plotter @@ -26,6 +26,8 @@ private: Serial* _serial; + bool highPrecision=false; + uint16_t diamondAngle(int16_t y, int16_t x); uint16_t calculateDelayTime(uint16_t currentDiamondAngle, uint16_t prevDiamondAngle, uint16_t prevSteps); @@ -36,10 +38,10 @@ public: void basicposition(); void pd(); void pu(); + void setHighPrecision(bool in); 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(); };