Mini

/* Mini CNC Arduino V1
 * Created 29 04 2019
 * by Papermindvention.blogspot.com
*/

#include <Stepper.h>
#include <Servo.h>

//stepper dvd-rom >>> 100 step = 15 mm >> 1 mm = 6.6 step <dari> 100 step / 15 mili == 6,6 step
#define SPR 20
#define SUp 100
#define SDown 179

char ValStr[80]; //Array untuk Menampung value String yang nantinya akan dirubah menjadi type data char
char *CharID; //
char *ValSX; //variabel yang akan menampung nilai untuk sumbuX
char *ValSY; //variabel yang akan menampung nilai untuk sumbuY
char *ValSX2; //variabel yang akan menampung nilai untuk sumbuX
char *ValSY2; //variabel yang akan menampung nilai untuk sumbuY

  //variable-variable string
  String inString = "";
  float posx = 0.0 ; //posisi sekarang
  float posy= 0.0; //posisi sekarang

Stepper SsumbuX(SPR, 2,3,4,5);
Stepper SsumbuY(SPR, 8,9,10,11);
Servo Zaxis;

void setup() {
  Serial.begin(9600);
  Zaxis.attach(6);
  SsumbuX.setSpeed(250);
  SsumbuY.setSpeed(250);
  Zaxis.write(SUp);
}


void loop() {
  while (Serial.available() > 0) {
    char inChar = Serial.read();
    inString += inChar;
    if (inChar == '\n') {
     for (int i=0; i< inString.length(); i++){ ValStr[i] = inString.charAt(i); } //proses merubah String menjadi character
      CharID = strtok (ValStr, " ");
      ValSX   = strtok (NULL, "X .");
      ValSX2  = strtok (NULL, " ");
      ValSY   = strtok (NULL, "Y .");
      ValSY2  = strtok (NULL, " ");
      moveXYZ(CharID);
    }
  }
}

//void moveXYZ(char*i1, char*i2){
void moveXYZ(char*i1){
  String ID = "";
      ID += i1;
      if (ID == "M03"){
          Zaxis.write(SDown);
          ID = "";
      } else if (ID == "M04"){
          Zaxis.write(SUp);
          ID = "";
      } if (ID == "G1" || ID == "G92"){
        float x = atoi(ValSX) + (atoi(ValSX2)/100.00);
        float y = atoi(ValSY) + (atoi(ValSY2)/100.00);

      //Formula
        float abx = (abs(x-posx)*6.0);
        float aby = (abs(y-posy)*6.0);
        int sstep = 1;
          int stepX =  posx > x ? -sstep : sstep;
          int stepY =  posy > y ? -sstep : sstep;
        long i;
        long over;
        if (abx > aby){
          for (i=0; i<abx; ++i){
            SsumbuX.step(stepX);
            over += aby;
            if (over >= abx){
              over -= abx;
              SsumbuY.step(stepY);
            }
          }
          posx = x;
          posy = y;
        } else if (aby > abx){
          for (i=0; i<aby; ++i){
            SsumbuY.step(stepY);
            over += abx;
            if (over >= aby){
              SsumbuX.step(stepX);
            }
          }
          posx = x;
          posy = y;
        } ID = "";
        } else if (ID.substring(0,2) == "G4"){
          delay(150);
          ID = "";
        } else {
          Serial.println("??");
          ID = "";
        } inString = "";
}