A small CNC

Forum 2005-2010 (read only) General Exhibition
6

Part 1

#include <WString.h>
#include <AFMotor.h>
#include <SoftwareSerial.h>

AF_Stepper motorx(48, 2);
AF_Stepper motory(48, 1);

#define rxPin 9
#define txPin 10
#define ledPin 13

SoftwareSerial mySerial =  SoftwareSerial(rxPin, txPin);

String inString = String(50); 
String units = String(2);  
String xstr = String(10);
String ystr = String(10);
String zstr = String(10);

float xstring;
float ystring;
float zstring;

float posx;
float posy;
float posz = min(posz, 0);

float targetx;
float targety;
float targetz;

float linex;
float liney;
float linez;

float DX;
float DY;
float DZ;

float dx;
float dy;
float dz;

float stepx = 341.759; // # of steps per mm
float stepy = 362.834;
float stepz = 121;

boolean command;
boolean zmark;
int type;
byte pinState = 0;
float scale = 1;
int zdirection = 1;

void setup() {
  Serial.begin(4800);
  motorx.setSpeed(60);
  motory.setSpeed(60);
  delay (1000);
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
  mySerial.begin(4800);
  command = false;
}

void AcquireXYZ(){
  if(Serial.available() > 0) {
 
   char inChar = Serial.read(); 
   
     if (inChar == '!'){   
       char firstChar = inString.charAt(0);
       
       if (firstChar == 'G'){ 
       GCodeParse();
       }
       
       else{
       }
       inString = 0;
     }
   else{
       inString.append(inChar); 
     }
  }
}

void GCodeParse(){
 
if (inString.startsWith("G0")){
        type = 0;
}
        
if (inString.startsWith("G1")){
        type = 1;
        if (inString.contains("X") && inString.contains("Y") && inString.contains("Z")){
        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;
        ycharloc = ycharloc - 1;
        xstr = inString.substring(xcharloc, ycharloc);
        ycharloc = ycharloc + 1;
        
        ycharloc = ycharloc + 1;
        zcharloc = zcharloc - 1;
        ystr = inString.substring(ycharloc, zcharloc);
        zcharloc = zcharloc + 1;

        int zlast = inString.length();
        zcharloc = zcharloc + 1;
        zstr = inString.substring(zcharloc, zlast);
        zcharloc = zcharloc - 1;
     
        xstring = atof(xstr);
        ystring = atof(ystr);
        zstring = atof(zstr);

        targetx = xstring * scale;
        targety = ystring * scale;
        targetz = -1 * zstring * zdirection * scale;
        
        DX = targetx - posx;
        DY = targety - posy;
        DZ = targetz - posz;
        linex = posx;
        liney = posy;
        delay (1000);
        command = true;
        };  
        if (inString.contains("X") && inString.contains("Y") && !inString.contains("Z")){
        int xcharloc = inString.indexOf('X'); //find where the X code starts
        int ycharloc = inString.indexOf('Y'); //find where the Y 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;
       
        int ylast = inString.length(); 
        ycharloc = ycharloc + 1;
        ystr = inString.substring(ycharloc, ylast);
        ycharloc = ycharloc - 1;

        xstring = atof(xstr); 
        ystring = atof(ystr); 
        
        targetx = xstring * scale;
        targety = ystring * scale;
        targetz = posz;//posz + DZ;
        
        DX = targetx - posx;
        DY = targety - posy;
        DZ = 0;
        linex = posx;
        liney = posy;
        delay (1000);
        command = true;
        };       
        if (inString.contains("X") && !inString.contains("Y") && inString.contains("Z")){
        int xcharloc = inString.indexOf('X'); 
        int zcharloc = inString.indexOf('Z'); 
       
        xcharloc = xcharloc + 1;
        zcharloc = zcharloc - 1;
        xstr = inString.substring(xcharloc, zcharloc);
        zcharloc = zcharloc + 1;
       
        int zlast = inString.length(); 
        zcharloc = zcharloc + 1;
        zstr = inString.substring(zcharloc, zlast);
        zcharloc = zcharloc - 1;

        xstring = atof(xstr);
        zstring = atof(zstr);

        targetx = xstring * scale;
        targety = posy;
        targetz = -1 * zstring * zdirection * scale;
        
        DX = targetx - posx;
        DY = 0;
        DZ = targetz - posz;
        linex = posx;
        liney = posy;
        delay (1000);
        command = true;
        };
        if (!inString.contains("X") && inString.contains("Y") && inString.contains("Z")){
        
        int ycharloc = inString.indexOf('Y');
        int zcharloc = inString.indexOf('Z');
       
        ycharloc = ycharloc + 1;
        zcharloc = zcharloc - 1;
        ystr = inString.substring(ycharloc, zcharloc);
        zcharloc = zcharloc + 1;

        int zlast = inString.length();
        zcharloc = zcharloc + 1;
        zstr = inString.substring(zcharloc, zlast);
        zcharloc = zcharloc - 1;

     
        ystring = atof(ystr);
        zstring = atof(zstr);

        targetx = posx;
        targety = ystring * scale;
        targetz = -1 * zstring * zdirection * scale;
        
        DX = 0;
        DY = targety - posy; 
        DZ = targetz - posz; 
        linex = posx;
        liney = posy;
        delay (1000);
        command = true;
        };      
        if (!inString.contains("X") && !inString.contains("Y") && inString.contains("Z")){
        
        
        int zcharloc = inString.indexOf('Z'); 
       
        int zlast = inString.length(); 
        zcharloc = zcharloc + 1;
        zstr = inString.substring(zcharloc, zlast);
        zcharloc = zcharloc - 1;
     
        zstring = atof(zstr);

        targetx = posx;
        targety = posy;
        targetz = -1 * zstring * zdirection * scale;
        
        DX = 0;
        DY = 0;
        DZ = targetz - posz;
        linex = posx;
        liney = posy;
        delay (1000);
        command = true;
        };
        if (!inString.contains("X") && inString.contains("Y") && !inString.contains("Z")){
        int ycharloc = inString.indexOf('Y');
        ycharloc = ycharloc + 1;
        int ylast = inString.length();
        ystr = inString.substring(ycharloc, ylast);
        ycharloc = ycharloc - 1;
        ystring = atof(ystr);
        targetx = posx;
        targety = ystring * scale;
        targetz = posz;
        
        DX = 0;
        DY = targety - posy;
        DZ = 0;
        linex = posx;
        liney = posy;
        delay (1000);
        command = true;
        };     
        if (inString.contains("X") && !inString.contains("Y") && !inString.contains("Z")){
        int xcharloc = inString.indexOf('X');
               
        int xlast = inString.length();
        xcharloc = xcharloc + 1;
        xstr = inString.substring(xcharloc, xlast);
        xcharloc = xcharloc - 1;
        xstring = atof(xstr);

        targetx = xstring * scale;
        targety = posy;
        targetz = posz;
        
        DX = targetx - posx;
        DY = 0;
        DZ = 0;
        linex = posx;
        liney = posy;
        delay (1000);
        command = true;
        };
}

}
void plotting(){
  if (type == 1){
  
  if (DX == 0 && DY != 0){
    DrawY();
    posz = targetz;
    Reportz();
  }  
  else if (DX != 0 && DY == 0){
    DrawX();
    posz = targetz;
    Reportz();
  }    
  else if (DX != 0 && DY != 0 && abs(DY) >= abs(DX)){
    DrawYX();
    posz = targetz;
    Reportz();
  }
  else if (DX != 0 && DY != 0 && abs(DX) > abs(DY)){
    DrawXY();
    posz = targetz;
    Reportz();
  }
  else if (DX == 0 && DY == 0){
    posz = targetz;
    Reportz();
    report(); 
    delay (abs(1000*scale));
  }  
 }
   report();
 }

void DrawY(){
  dy = 1 ;
     if (DY > 0) {
       if (posy < targety ) {
        motory.step(dy, BACKWARD, SINGLE);
        posx = posx;
        posy = posy + dy / stepy;
        report();
       }
     }
      else {
        if (posy > targety ){
        motory.step(dy, FORWARD, SINGLE);
        posx = posx;
        posy = posy - dy / stepy;
        report();
      }
    }
}

void DrawX(){
  dx = 1;
      if (DX > 0) {
        if (posx < targetx) {
        motorx.step(dx, BACKWARD, SINGLE);
        posx = posx + dx / stepx;
        posy = posy;
        report();
        }
      }
      else {
        if (posx > targetx){
        motorx.step(dx, FORWARD, SINGLE);
        posx = posx - dx / stepx;
        posy = posy;
        report();
      }
    }
}