/cnc.ino

//#include <LiquidCrystal.h>
//#include <Servo.h> 
#include <TimerOne.h>


//Servo myservo;
int pos = 0;    // variable to store the servo position

int  axisspeed = 10;
int  zaxisspeed = 10;
int extrudspeed= 500;

int xcoila1 = 2;
int xcoila2 = 3;
int xcoilb1 = 4;
int xcoilb2 = 5;


int ycoila1 = 6;
int ycoila2 = 7;
int ycoilb1 = 8;
int ycoilb2 = 9;


int zcoila1 = 10;
int zcoila2 = 11;
int zcoilb1 = 12;
int zcoilb2 = 13;

int ecoila1 = 46;
int ecoila2 = 48;
int ecoilb1 = 50;
int ecoilb2 = 52;

int x=0;
int y=0;
int z=0;

int extrudOn = 0;


//int xdir=1;
//int ydir=0;
//int zdir=0;

//Servo myservo;

String inputString = "";         // a string to hold incoming data
String xstr = "";
String ystr = "";
String zstr = ""; 
String tempstr = "";
String extrudestr = "";

int Sxlimit1 = HIGH;
int Sylimit1 = HIGH;

int Sxlimit2 = HIGH;
int Sylimit2 = HIGH;

int homed = 0;

int limiterror = 0;

int curTemp = 0;

int tempSet = 29;

int heatPin = 40;

double tempC;


boolean stringComplete = false;  // whether the string is complete

int turn = 0;
 
void setup()
{
  
  
   Timer1.initialize(100000); // set a timer of length 100000 microseconds (or 0.1 sec - or 10Hz => the led will blink 5 times, 5 cycles of on-and-off, per second)
   Timer1.attachInterrupt( extrudturn ); // attach the service routine here
  
  // myservo.attach(52);
   //myservo.write(90);
   
   
   pinMode(21, INPUT); 
   pinMode(20, INPUT); 
   pinMode(19, INPUT); 
   pinMode(18, INPUT);
   
   pinMode(heatPin, OUTPUT);
  
   Sylimit1 = digitalRead(21);
   Sxlimit1 = digitalRead(20);
   Sylimit2 = digitalRead(19);
   Sxlimit2 = digitalRead(18);
   
   
  //myservo.attach(22);
  
  attachInterrupt(2, ylimit1On, LOW);
  attachInterrupt(4, ylimit2On, LOW);
  
  attachInterrupt(3, xlimit1On, LOW);
  attachInterrupt(5, xlimit2On, LOW);
  
  pinMode(xcoila1, OUTPUT);
  pinMode(xcoila2, OUTPUT);
  pinMode(xcoilb1, OUTPUT);
  pinMode(xcoilb2, OUTPUT);
  
  pinMode(ycoila1, OUTPUT);
  pinMode(ycoila2, OUTPUT);
  pinMode(ycoilb1, OUTPUT);
  pinMode(ycoilb2, OUTPUT);
  
  pinMode(zcoila1, OUTPUT);
  pinMode(zcoila2, OUTPUT);
  pinMode(zcoilb1, OUTPUT);
  pinMode(zcoilb2, OUTPUT);
  
  pinMode(ecoila1, OUTPUT);
  pinMode(ecoila2, OUTPUT);
  pinMode(ecoilb1, OUTPUT);
  pinMode(ecoilb2, OUTPUT);
  
  
  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);
  
  xIdle();
  yIdle();
  zIdle();
  eIdle();  
 
  
  //axisTest();
  homeing();

  
  Serial.begin(9600);
  inputString.reserve(200);
  xstr.reserve(5);
  ystr.reserve(5);
  zstr.reserve(5);
  tempstr.reserve(5);
  extrudestr.reserve(5);
  
  
    
  prepareHeater();
  

  //myservo.write(50);
  
  Serial.flush();
  
  Serial.println("Begin");
  
  
  
}


double Thermister(int RawADC) {
  double Temp;
  Temp = log(((10240000/RawADC) - 10000));
  Temp = 1 / (0.001129148 + (0.000234125 * Temp) + (0.0000000876741 * Temp * Temp * Temp));
  Temp = Temp - 273.15;           // Convert Kelvin to Celcius
  return Temp;
}


void axisTest(){
        
  for (int i=0;i<50;i++){
   
    zbackward();
    
  }
  
  
  for (int i=0;i<100;i++){
   
    xforward();
    
  }
  
  
  for (int i=0;i<100;i++){
   
    xbackward();
    
  }
  
   
  for (int i=0;i<100;i++){
   
    yforward();
    
  }
  
  
  for (int i=0;i<100;i++){
   
    ybackward();
    
  }
  
  for (int i=0;i<50;i++){
   
    zforward();
    
  } 
  
  
}

void xIdle(){
  
      digitalWrite(xcoila1, LOW);
      digitalWrite(xcoila2, LOW);
      digitalWrite(xcoilb1, LOW);
      digitalWrite(xcoilb2, LOW);
} 

void yIdle(){
      digitalWrite(ycoila1, LOW);
      digitalWrite(ycoila2, LOW);
      digitalWrite(ycoilb1, LOW);
      digitalWrite(ycoilb2, LOW);
}

void zIdle(){
      digitalWrite(zcoila1, LOW);
      digitalWrite(zcoila2, LOW);
      digitalWrite(zcoilb1, LOW);
      digitalWrite(zcoilb2, LOW); 
  
}


void eIdle(){
      digitalWrite(ecoila1, LOW);
      digitalWrite(ecoila2, LOW);
      digitalWrite(ecoilb1, LOW);
      digitalWrite(ecoilb2, LOW); 
  
}
void homeing(){
  
   
      while ( Sxlimit1 == HIGH ){
           
           xbackward(); 
      }

      while ( Sylimit1 == HIGH ){
           
          ybackward(); 
     }
     
    
     for (int i=0;i<350;i++){
   
        xforward();
    
     }
     
     for (int i=0;i<350;i++){
   
        yforward();
    
     }
    
    Sylimit1 = HIGH;
    Sxlimit1 = HIGH;
    
    delay(100);
     
}

void prepareHeater(){
  
      heatOn();
  
        tempC = Thermister(analogRead(0));  // Read sensor
 
      while((int)tempC < tempSet){
        
       tempC = Thermister(analogRead(0));  // Read sensor
        Serial.println(" TEMP :" + String(tempSet) + " Cur TEMP:" + String ((int)tempC));
        delay(100);
        
      }
         
  
} 


void eforward(){
  
  digitalWrite(ecoila1, LOW);
  digitalWrite(ecoila2, LOW);
  digitalWrite(ecoilb1, LOW);
  digitalWrite(ecoilb2, LOW);
  
  if (extrudOn==0){
    return ; 
  }
  
  if (turn > 3){
    turn = 0; 
  }
  
  if(turn == 0){
    digitalWrite(ecoila1, HIGH);
  }
  
  if(turn == 1){
    digitalWrite(ecoila2, HIGH);
  }
  if(turn == 2){
    digitalWrite(ecoilb1, HIGH);
  }
  if(turn == 3){
    digitalWrite(ecoilb2, HIGH);
  }
  
  turn++;
  
}

void ebackward(){
  
  digitalWrite(ecoila1, LOW);
  digitalWrite(ecoila2, LOW);
  digitalWrite(ecoilb1, LOW);
  digitalWrite(ecoilb2, LOW);
  
  
  if (extrudOn==0){
    return ; 
  }
  
  if (turn > 3){
    turn = 0; 
  }
  
  if(turn == 3){
    digitalWrite(ecoila1, HIGH);
  }
  
  if(turn == 2){
    digitalWrite(ecoila2, HIGH);
  }
  if(turn == 1){
    digitalWrite(ecoilb1, HIGH);
  }
  if(turn == 0){
    digitalWrite(ecoilb2, HIGH);
  }
  
  turn++;
  
}



void extrudturn(){
 
   ebackward();
  
}




 
 


void ybackward(){
  
  digitalWrite(xcoila1, HIGH);
  digitalWrite(xcoila2, LOW);
  digitalWrite(xcoilb1, LOW);
  digitalWrite(xcoilb2, LOW);
  delay(axisspeed);
  
 
  digitalWrite(xcoila1, LOW);
  digitalWrite(xcoila2, HIGH);
  digitalWrite(xcoilb1, LOW);
  digitalWrite(xcoilb2, LOW);
  delay(axisspeed);
  
 
  digitalWrite(xcoila1, LOW);
  digitalWrite(xcoila2, LOW);
  digitalWrite(xcoilb1, HIGH);
  digitalWrite(xcoilb2, LOW);
  delay(axisspeed);
 
 
  digitalWrite(xcoila1, LOW);
  digitalWrite(xcoila2, LOW);
  digitalWrite(xcoilb1, LOW);
  digitalWrite(xcoilb2, HIGH);
  delay(axisspeed);
  
}



 
 
void yforward(){
  
  
 
  digitalWrite(xcoila1, LOW);
  digitalWrite(xcoila2, LOW);
  digitalWrite(xcoilb1, LOW);
  digitalWrite(xcoilb2, HIGH);
  delay(axisspeed);
  
  
  digitalWrite(xcoila1, LOW);
  digitalWrite(xcoila2, LOW);
  digitalWrite(xcoilb1, HIGH);
  digitalWrite(xcoilb2, LOW);
  delay(axisspeed);
  
  
  digitalWrite(xcoila1, LOW);
  digitalWrite(xcoila2, HIGH);
  digitalWrite(xcoilb1, LOW);
  digitalWrite(xcoilb2, LOW);
  delay(axisspeed);
 
  
  digitalWrite(xcoila1, HIGH);
  digitalWrite(xcoila2, LOW);
  digitalWrite(xcoilb1, LOW);
  digitalWrite(xcoilb2, LOW);
  delay(axisspeed);
  
}



void xbackward(){
  
  digitalWrite(ycoila1, HIGH);
  digitalWrite(ycoila2, LOW);
  digitalWrite(ycoilb1, LOW);
  digitalWrite(ycoilb2, LOW);
  delay(axisspeed);
  
 
  digitalWrite(ycoila1, LOW);
  digitalWrite(ycoila2, HIGH);
  digitalWrite(ycoilb1, LOW);
  digitalWrite(ycoilb2, LOW);
  delay(axisspeed);
  
 
  digitalWrite(ycoila1, LOW);
  digitalWrite(ycoila2, LOW);
  digitalWrite(ycoilb1, HIGH);
  digitalWrite(ycoilb2, LOW);
  delay(axisspeed);
 
 
  digitalWrite(ycoila1, LOW);
  digitalWrite(ycoila2, LOW);
  digitalWrite(ycoilb1, LOW);
  digitalWrite(ycoilb2, HIGH);
  delay(axisspeed);
  
}

 
 
void xforward(){
  
  
 
  digitalWrite(ycoila1, LOW);
  digitalWrite(ycoila2, LOW);
  digitalWrite(ycoilb1, LOW);
  digitalWrite(ycoilb2, HIGH);
  delay(axisspeed);
  
  
  digitalWrite(ycoila1, LOW);
  digitalWrite(ycoila2, LOW);
  digitalWrite(ycoilb1, HIGH);
  digitalWrite(ycoilb2, LOW);
  delay(axisspeed);
  
  
  digitalWrite(ycoila1, LOW);
  digitalWrite(ycoila2, HIGH);
  digitalWrite(ycoilb1, LOW);
  digitalWrite(ycoilb2, LOW);
  delay(axisspeed);
 
  
  digitalWrite(ycoila1, HIGH);
  digitalWrite(ycoila2, LOW);
  digitalWrite(ycoilb1, LOW);
  digitalWrite(ycoilb2, LOW);
  delay(axisspeed);
  
}


/*void zforward(){
  
    myservo.write(19);
  
}

void zbackward(){
  
    myservo.write(30);
  
}*/


void zforward(){
  
  digitalWrite(zcoila1, HIGH);
  digitalWrite(zcoila2, LOW);
  digitalWrite(zcoilb1, LOW);
  digitalWrite(zcoilb2, LOW);
  delay(zaxisspeed);
  
 
  digitalWrite(zcoila1, LOW);
  digitalWrite(zcoila2, HIGH);
  digitalWrite(zcoilb1, LOW);
  digitalWrite(zcoilb2, LOW);
  delay(zaxisspeed);
  
 
  digitalWrite(zcoila1, LOW);
  digitalWrite(zcoila2, LOW);
  digitalWrite(zcoilb1, HIGH);
  digitalWrite(zcoilb2, LOW);
  delay(zaxisspeed);
 
 
  digitalWrite(zcoila1, LOW);
  digitalWrite(zcoila2, LOW);
  digitalWrite(zcoilb1, LOW);
  digitalWrite(zcoilb2, HIGH);
  delay(zaxisspeed);
  
}

 


void zbackward(){
  
  digitalWrite(zcoila1, LOW);
  digitalWrite(zcoila2, LOW);
  digitalWrite(zcoilb1, LOW);
  digitalWrite(zcoilb2, HIGH);
  delay(zaxisspeed);
  
 
  digitalWrite(zcoila1, LOW);
  digitalWrite(zcoila2, LOW);
  digitalWrite(zcoilb1, HIGH);
  digitalWrite(zcoilb2, LOW);
  delay(zaxisspeed);
  
 
  digitalWrite(zcoila1, LOW);
  digitalWrite(zcoila2, HIGH);
  digitalWrite(zcoilb1, LOW);
  digitalWrite(zcoilb2, LOW);
  delay(zaxisspeed);
 
 
  digitalWrite(zcoila1, HIGH);
  digitalWrite(zcoila2, LOW);
  digitalWrite(zcoilb1, LOW);
  digitalWrite(zcoilb2, LOW);
  delay(zaxisspeed);
  
}

 


void strgcode(){
  
    int i=0;
    int axis=-1;
    int op = -1;
    
  
  
    while(inputString[i] != '\0'){
      
    
      if(inputString[i]==' '){
        i++;
        continue; 
        
      }
      
      if(inputString[i]=='X'){
        axis=0;
        i++;
        continue;
      }  
      
      if(inputString[i]=='Y'){
        axis=1;
        i++;
        continue;
      }
      
      if(inputString[i]=='Z'){
        axis=2;
        i++;
        continue;
      }
      
      if(inputString[i]=='S'){
        op=0;
        i++;
        continue;
      }
      
      if(inputString[i]=='M'){
        op=1;
        i++;
        continue;
      }
      
      if(axis==0){
       
        xstr+=inputString[i];
        
      }
      
      if(axis==1){
       
        ystr+=inputString[i];
        
      }

      if(axis==2){
       
        zstr+=inputString[i];
        
      } 

      if(op==0){
       
        tempstr+=inputString[i];
        
      }        
      
      if(op==1){
       
        extrudestr+=inputString[i];
        
      }        
      
      i++;
      
    }
  
  
    
  
}

void  heatOn(){
  
  
  digitalWrite(heatPin, HIGH);
  
  
}

void heatOff(){
  
    digitalWrite(heatPin, LOW);
  
}

 
void loop()
{
  //digitalWrite(13, HIGH);
  //delay(2000);
  //digitalWrite(13, LOW);
  
  tempC = Thermister(analogRead(0));  // Read sensor
      

      if((int)tempC < tempSet+3){
        
         heatOn();
        
      }else{
        
            if((int)tempC > tempSet+10){
             
              heatOff();
            }
  
      }

  //Serial.println("Idle "+ String(x) +" "+ String(y)+" "+ String(z) + " TEMP :" + String(tempSet) + " Cur TEMP:" + String ((int)tempC));
  
 if (stringComplete) {
   
   
    /*Serial.println(inputString);
       // clear the string:
    inputString = "";
    stringComplete = false; */
    
    strgcode();
    Serial.flush();
    
    int xdone=0;
    int ydone=0;
    int zdone=0;
    
     if(tempstr.compareTo("")!= 0){
        tempSet = tempstr.toInt();
      }
      
    if(extrudestr.compareTo("")!= 0){
          if(extrudestr.toInt() == 101){
                  extrudOn = 1;
    
        }else{
           
            if(extrudestr.toInt()==103){ 
              
                extrudOn = 0;
            }
        }
    }
       
    
    //digitalWrite(13, Sxlimit1 );
    
    //if(tempstr.toInt())
   
    
    while(xdone==0 || ydone==0 || zdone==0){
      
  
      
      
      if (Sxlimit1 == LOW || Sylimit1 == LOW || Sxlimit2 == LOW || Sylimit2 == LOW ){
       
        xIdle();
        yIdle();
        zIdle();
        heatOff();
        
         Serial.println("Error "+ String(Sxlimit1)+" "+String(Sylimit1)+" "+ String(Sxlimit2)+" "+String(Sylimit2)); 
        
         while(1){
           
         }
      }
      
    
       
      
       if(xstr.compareTo("")!= 0){
           if(x<xstr.toInt()){
              
             xforward(); 
             x++;  
           }else{
           
               if(x>xstr.toInt()){ 
                 xbackward();
                 x--;   
               }else{ 
                 xdone=1;
               }
               
           }
       
       }else{
          xdone=1; 
       }
     
     
     if(ystr.compareTo("")!= 0){
       if(y<ystr.toInt()){ 
           yforward();
           y++;   
         }else{
         
             if(y>ystr.toInt()){ 
               ybackward();
               y--;   
             }else{ 
               ydone=1;
             }
             
         }
     }else{
       
       ydone=1; 
       
     }


      if(zstr.compareTo("")!= 0){
          if(z<zstr.toInt()){ 
            
         for (int i=0;i<10;i++){
   
            zforward();
    
          }
            
            
             
             z++;   
           }else{
           
               if(z>zstr.toInt()){ 
                  
                   for (int i=0;i<10;i++){
   
                      zbackward();
    
                   }
                  
                 z--;   
               }else{ 
                 zdone=1;
               }
               
           } 
  
      }else{
   
          zdone=1;
      }     
      
       
   }
     
 
   Serial.println("Done "+ String(xstr) +" "+ String(ystr)+" "+ String(zstr) + " TEMP :" + String(tempSet) + "Cur TEMP:" + String ((int)tempC)); 
 
    // clear the string:
    inputString = "";
    xstr="";
    ystr="";
    zstr="";
    tempstr="";
    extrudestr="";
    stringComplete = false;
    

  }
  
  
 
    
}


void xlimit1On(){
  
  Sxlimit1 = !Sxlimit1;
  
}




void ylimit1On(){
  
  Sylimit1 = !Sylimit1;
}




void xlimit2On(){
  
  Sxlimit2 = !Sxlimit2;
  
}




void ylimit2On(){
  
  Sylimit2 = !Sylimit2;
}



void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read(); 
    // add it to the inputString:
    if (inChar == '\n') {
      
      inputString+='\0';
      stringComplete = true;
      
      return;
    } 
    
    inputString += inChar;
    // if the incoming character is a newline, set a flag
    // so the main loop can do something about it:
    
  }
}