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

Motion Control Updates

Browse source
This commit is contained in:
IMback 2017-09-22 21:40:29 +02:00
parent 471d018bfe
commit cc1bdaa82f
5 changed files with 132 additions and 41 deletions
Download patch file Download diff file Expand all files Collapse all files

108
plotter.cpp
View file

@ -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
int steps = 0;
int32_t StepSizeX=0;
int32_t StepSizeY=0;
/*_serial->write("deltaX: ");
_serial->write(deltaX);
_serial->write(" deltaY: ");
_serial->write(deltaY);*/
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;
}
else
{
StepSizeX = (((int32_t)abs(deltaX)*100)/abs((int32_t)deltaY))*(deltaX/abs(deltaX));
StepSizeY = 100*(deltaY/abs(deltaY));
steps = deltaY/StepSizeY;
}
uint16_t steps = 0; //one step is max 1.28205mm min 0.8547mm
for( int i = 0; i < steps; i++)
if(deltaX != 0 || deltaY != 0)
{
_pwm.setDutyA(65535-(currentPos.x+i*StepSizeX)*SCALEX);
_pwm.setDutyB(65535-(currentPos.y+i*StepSizeY)*SCALEY);
_delay_us(5000);
int32_t StepSizeX=0;
int32_t StepSizeY=0;
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;
}
else
{
if(deltaX != 0) StepSizeX = ((int32_t)abs(deltaX)*100)/abs(deltaY)*sgn(deltaX);
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');
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(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;