102 lines
2 KiB
C++
102 lines
2 KiB
C++
#include "plotter.h"
|
|
|
|
const int SCALEX=2;
|
|
const int SCALEY=3;
|
|
|
|
Plotter::Plotter(volatile unsigned char *penPort, const char penPin): _pwm( &TCCR1A, &TCCR1B, &OCR1A, &OCR1B, &ICR1, 0b00000001)
|
|
{
|
|
_penPort=penPort;
|
|
_penPin=penPin;
|
|
basicposition();
|
|
}
|
|
|
|
Point Plotter::getCurrentPos()
|
|
{
|
|
return currentPos;
|
|
}
|
|
|
|
|
|
void Plotter::demo()
|
|
{
|
|
moveto(65535,0);
|
|
moveto(65535,42129);
|
|
moveto(0,42129);
|
|
moveto(0,0);
|
|
pd();
|
|
pu();
|
|
}
|
|
|
|
void Plotter::basicposition()
|
|
{
|
|
moveto(0, 0);
|
|
}
|
|
|
|
void Plotter::pd()
|
|
{
|
|
if(!readPin(_penPort, _penPin))
|
|
{
|
|
writePin(_penPort, _penPin, true);
|
|
_delay_us(500);
|
|
}
|
|
}
|
|
|
|
void Plotter::pu()
|
|
{
|
|
if(readPin(_penPort, _penPin))
|
|
{
|
|
writePin(_penPort, _penPin, false);
|
|
_delay_us(500);
|
|
}
|
|
}
|
|
|
|
void Plotter::moveto(Point *pt)
|
|
{
|
|
moveto((*pt).x,(*pt).y);
|
|
}
|
|
|
|
void Plotter::moveto(const uint16_t nx, const uint16_t ny)
|
|
{
|
|
int deltaX = nx - currentPos.x; //-5100
|
|
int deltaY = ny - currentPos.y; //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;
|
|
}
|
|
else
|
|
{
|
|
StepSizeX = (((int32_t)abs(deltaX)*100)/abs((int32_t)deltaY))*(deltaX/abs(deltaX));
|
|
StepSizeY = 100*(deltaY/abs(deltaY));
|
|
steps = deltaY/StepSizeY;
|
|
}
|
|
|
|
for( int i = 0; i < steps; i++)
|
|
{
|
|
_pwm.setDutyA(65535-(currentPos.x+i*StepSizeX)*SCALEX);
|
|
_pwm.setDutyB(65535-(currentPos.y+i*StepSizeY)*SCALEY);
|
|
_delay_us(5000);
|
|
}
|
|
|
|
|
|
_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;
|
|
}
|
|
|