I am writing a G-code interpreter and have run into a problem. When the X and Y axis get to their desired position (Right now I just have it return the axis and a .01 unit movement over serial) they just move +0.01 and -0.01 back and forth.
How can I get it to stop it when it reaches the desired position?
#include <WString.h> // include the String library
String inString = String(50); // allocate a new String for the incoming group of chars.
String units = String(2); // allocate a new String for the units setting. Inches or Milimeters.
String xstr = String(10); // string for the X caricters to be dumped before thy become and double or float.
String ystr = String(10); // string for the Y caricters to be dumped before thy become and double or float.
String zstr = String(10); // string for the Z caricters to be dumped before thy become and double or float.
double xCurrent = 0;
double yCurrent = 0;
double zCurrent = 0;
double xDouble;
double yDouble;
double zDouble;
double xDiference;
double yDiference;
double Diference;
void setup() {
Serial.begin(9600); // open the serial port
Serial.println("start"); // Say hello
}
void loop () {
if(Serial.available() > 0) { //add chars if they are avalible
char inChar = Serial.read(); // read the incoming data as a char
if (inChar == 'l'){ // If it is the end of the string decode the string and do the actions.
char firstChar = inString.charAt(0);
//start parsing string
if (firstChar == 'G'){ //Is it a G-code
GCodeParse();
}
else if (firstChar == 'M'){//Is it an M-code
MCodeParse();
}
else{//If it is nether a G or M code send to the computer to continue.
Serial.println("continue");
}
//stop parsing and erase string
inString = 0;
}
else{
inString.append(inChar); // if you're not at the end of the string, append the incoming character
}
}
}
void MCodeParse(){
if (inString.equals("M101")){
Serial.println("Extruder on");
//add code for turning on extruder ---------------------------------------------------<<<
Serial.println("continue"); // Tell the computer to go on.
}
else if (inString.equals("M103")){
Serial.println("Extruder off");
//add code for extruder off ---------------------------------------------------<<<
Serial.println("continue"); // Tell the computer to go on.
}
else if (inString.equals("M106")){
Serial.println("Fan on");
//add code for fan on ---------------------------------------------------<<<
Serial.println("continue"); // Tell the computer to go on.
}
else if (inString.equals("M107")){
Serial.println("Fan off");
//add code for fan of ---------------------------------------------------<<<
Serial.println("continue"); // Tell the computer to go on.
}
// ----------- do we need extruder P,I,and D gain? Do we need set temp or extrude speed -------------
else{
Serial.println("continue"); // If unknow code tell the computer to go on.
}
}
void GCodeParse(){
if (inString.startsWith("G0 ") || inString.startsWith("G1 ")){ // Is the code for positioing?
//Code for finding X Y and Z cordenents
int xcharloc = inString.indexOf('X'); //find where the X code starts
int ycharloc = inString.indexOf('Y'); //find where the Y code starts
int zcharloc = inString.indexOf('Z'); //find where the Z code starts
xcharloc = xcharloc + 1;// Code for finding what X is in the code
ycharloc = ycharloc - 1;
xstr = inString.substring(xcharloc, ycharloc);
ycharloc = ycharloc + 1;
ycharloc = ycharloc + 1; // Code for finding what Y is in the code
zcharloc = zcharloc - 1;
ystr = inString.substring(ycharloc, zcharloc);
zcharloc = zcharloc + 1;
int zlast = inString.length(); // Code for finding what z is in the code, This is some more complex code
if(inString.contains("F")){
int fcharloc = inString.indexOf('F'); //find where the F code starts
zcharloc = zcharloc + 1;
fcharloc = fcharloc - 1;
zstr = inString.substring(zcharloc, fcharloc);
zcharloc = zcharloc - 1;
fcharloc = fcharloc + 1;
}else{
zcharloc = zcharloc + 1;
zstr = inString.substring(zcharloc, zlast);
Serial.print(zstr);
Serial.println(" <Z value");
zcharloc = zcharloc - 1;
}
xDouble = atof(xstr); //convert string with x cordanents to double format
yDouble = atof(ystr); //convert string with y cordanents to double format
zDouble = atof(zstr); //convert string with z cordanents to double format
Serial.print("X as a double : ");
Serial.println(xDouble);
Serial.print("Y as a double : ");
Serial.println(yDouble);
Serial.print("Z as a double : ");
Serial.println(zDouble);
if(zCurrent == zDouble){ // If a new Z position is not requested do nothing.
}else{//if it is requested move the axie
while(zCurrent < zDouble){ //Do we move up?
//Move tward zDouble by 0.01 units
Serial.print("z+");
zCurrent = zCurrent + 0.01; //write in current location
}
while(zCurrent > zDouble){ //Or do we move down?
//Move tward zDouble by 0.01 units
Serial.print("z-");
zCurrent = zCurrent - 0.01; //write in current location
}
}
move();
Serial.println("continue"); // Tell the computer to go on when you are done.
}
else if (inString.startsWith("G20 ")){
units = "in";
Serial.println("continue"); // Tell the computer to go on.
}
else if (inString.startsWith("G21 ")){
units = "mm";
Serial.println("continue"); // Tell the computer to go on.
}
// ---------------------------???Do we need "G28, G30, G90, G91, G92"??? Arcs? Dwells?--------------------------
else{
Serial.println("continue"); // If unknow code tell the computer to go on.
}
}
void xBig(){
}
void yBig(){
}
void xSmall(){
if(xDouble == xCurrent){
//do nothing!!!
}else{
if(xCurrent < xDouble){ //Do we move up?
Serial.print("x+"); //Move tward zDouble by 0.01 units
xCurrent = xCurrent + 0.01; //write in current location
}else{
Serial.print("x-"); //Move tward zDouble by 0.01 units
xCurrent = xCurrent - 0.01; //write in current location
}
}
}
void ySmall(){
if(yDouble == yCurrent){
//do nothing!!!
}else{
if(yCurrent < yDouble){ //Do we move up?
Serial.print("y+"); //Move tward zDouble by 0.01 units
yCurrent = yCurrent + 0.01; //write in current location
}else{
Serial.print("y-"); //Move tward zDouble by 0.01 units
yCurrent = yCurrent - 0.01; //write in current location
}
}
}
void move(){
while(xDouble != xCurrent||yDouble != yCurrent);{
// Find the difference
if(yDiference > 0 && xDiference > 0){
if(xDouble > xCurrent){
xDiference = xDouble - xCurrent;
}else{
xDiference = xCurrent - xDouble;
}
if(yDouble > yCurrent){
yDiference = yDouble - yCurrent;
}else{
yDiference = yCurrent - yDouble;
}
if(xDiference > yDiference){
Diference = xDiference/yDiference;
}else{
Diference = yDiference/xDiference;
} // STOP finding the difference
}else{
}
Serial.print(Diference); // Tell the godfather what the difference is.
Serial.print("X: ");
Serial.print(xCurrent);
Serial.print("Y: ");
Serial.println(yCurrent);
if(xDiference > yDiference){
xSmall();
ySmall();
}else{
xSmall();
ySmall();
}
}
}
The Part I am having trouble with is the void xSmall(), ySmall(), and move().
