114 lines
2.3 KiB
C
114 lines
2.3 KiB
C
#ifndef UTILS_H_INCLUDED
|
|
#define UTILS_H_INCLUDED
|
|
|
|
unsigned long int mod(long int a){
|
|
return a>0?a:-a;
|
|
}
|
|
|
|
double toAbsoluteX(double x){
|
|
return posX+x;
|
|
}
|
|
|
|
double toAbsoluteY(double y){
|
|
return posY+y;
|
|
}
|
|
|
|
String removeComments(String code){
|
|
if(code.indexOf(";")!=-1){
|
|
return code.substring(0, code.indexOf(";"));
|
|
}
|
|
else if(code.indexOf("(")!=-1){
|
|
return code.substring(0, code.indexOf("("))+code.substring(code.indexOf(")"));
|
|
}
|
|
else{
|
|
return code;
|
|
}
|
|
}
|
|
|
|
double getParameter(String code, char param){
|
|
char b[50];
|
|
code.substring(code.indexOf(param)+1, code.indexOf(" ", code.indexOf(param))).toCharArray(b, 50);
|
|
return atof(b);
|
|
}
|
|
|
|
String getCommand(String code){
|
|
return code.substring(0, code.indexOf(" "));
|
|
}
|
|
|
|
void goTo(double x, double y, long speed){
|
|
/**
|
|
Makes use of Bresenham algorithm
|
|
@link https://www.marginallyclever.com/blog/2013/08/how-to-build-an-2-axis-arduino-cnc-gcode-interpreter/
|
|
**/
|
|
|
|
//set speed
|
|
stepperX.setSpeed(speed);
|
|
stepperY.setSpeed(speed);
|
|
|
|
//convert to something "normal"
|
|
if(relative){
|
|
x=toAbsoluteX(x);
|
|
y=toAbsoluteY(y);
|
|
}
|
|
|
|
//maintain limits
|
|
if(MAX_X!=-1 && x>MAX_X){
|
|
x=MAX_X;
|
|
}
|
|
if(MAX_Y !=-1 && y>MAX_Y){
|
|
y=MAX_Y;
|
|
}
|
|
|
|
//coords to steps
|
|
int xsteps=x*STEPX;
|
|
int ysteps=y*STEPY;
|
|
|
|
//calculate deltas
|
|
int dx=abs(xsteps-posX);
|
|
int dy=abs(ysteps-posY);
|
|
int moveX=x>posX?STEP_INCREMENT:-STEP_INCREMENT;
|
|
int moveY=y>posY?STEP_INCREMENT:-STEP_INCREMENT;
|
|
|
|
unsigned long int over=0;
|
|
|
|
if(dx>dy){
|
|
for(unsigned long int i=0; i<dx; i++){
|
|
stepperX.step(moveX);
|
|
over+=dy;
|
|
if(over>=dx){
|
|
over-=dx;
|
|
stepperY.step(moveY);
|
|
}
|
|
delay(STEP_DELAY);
|
|
}
|
|
}
|
|
else{
|
|
for(unsigned long int i=0; i<dy; i++){
|
|
stepperY.step(moveY);
|
|
over+=dx;
|
|
if(over>=dy){
|
|
over-=dy;
|
|
stepperX.step(moveX);
|
|
}
|
|
delay(STEP_DELAY);
|
|
}
|
|
}
|
|
|
|
//update coords
|
|
posX=x;
|
|
posY=y;
|
|
posXmm=x;
|
|
posYmm=y;
|
|
}
|
|
|
|
void penUp(int pen, bool up=true){
|
|
if(up){
|
|
digitalWrite(pen_pins[pen], HIGH);
|
|
}
|
|
else{
|
|
digitalWrite(pen_pins[pen], LOW);
|
|
}
|
|
}
|
|
|
|
#endif // UTILS_H_INCLUDED
|