// ArduinoCNC
// Styroporschneidemaschine über serielle Schnittstelle gesteuert 
// Version: 1.4.0
// Datum: 17.Mai.14 
// Versionskommentar: Masterclock auf Null gesetzt bei Initialisierung
// -----------------------------------------------------------------------------------------
// Hinweis: Arduino IDE muss auf Board "Diecimila" eingestellt sein
// -----------------------------------------------------------------------------------------
// Befehle, die über die Serielle Schnittstelle empfangen werden:
//  '5' - Links Y-Achse nach oben
//  '6' - Links Y-Achse nach unten
//  '7' - Links X-Achse nach vorne
//  '8' - Links X-Achse nach hinten
//  '1' - Rechts Y-Achse nach oben
//  '2' - Rechts Y-Achse nach unten
//  '3' - Rechts X-Achse nach vorne
//  '4' - Rechts X-Achse nach hinten
//  '0' - Stoppt die Bewegung aller Achsen
//  'S' - Startet den Schneidevorgang. Ab dann holt sich der Arduino die Daten ab
//  "<DIR><hex>" - bewegt beide Seiten um <hex> Schritte
//                 <DIR> = 'U'  Aufwärts
//                 <DIR> = 'D'  Abwärts
//                 <DIR> = 'F'  Vorwärts
//                 <DIR> = 'B'  Rückwärts
//                 Beispiel: F00F3
//  "<XY><sign><StepHex><sign><XaspectHex><sign>YaspectHex>"
//                 - Übergibt einen Vector an den Arduino. Der Arduino weiß, ob für die rechte
//                   oder linke Seite, weil er den Vector vorher entsprechend angefragt hat. 
//                   Die drei Vorzeichen geben die Richtung an.
//                   Die drei Werte sind unsigned Hex (4 Characters).
//                   StepHex - Anzahl der Schritte in X-Richtung
//                   XAspectHex - Alle wieviel Clocks wird ein X-Motorschritt gemacht.
//                   YAspectHex - Alle wieviel Clocks wird ein Y-Motorschritt gemacht.
//                   Beispiel: X+024E+0041-003D
// "##----#----#----" - Keine weiteren Vectoren mehr d.h. Arduino hört auf danach zu fragen.
//
// Befehle, die gesendet werden:
// 'L' - Arduino fordert nächsten Vector für linke Seite an
// 'R' - Arduino fordert nächsten Vector für rechte Seite an
// 'A' - Rechter Endschalter in X-Richtung erreicht
// 'B' - Linker Endschalter in X-Richtung erreicht
// 'a' - Rechter Endschalter in Y-Richtung erreicht
// 'b' - Linker Endschalter in Y-Richtung erreicht


int xrDir=0, xlDir=0, yrDir=0, ylDir=0;
boolean bitXR0, bitXR1, bitYR0, bitYR1;
boolean bitXL0, bitXL1, bitYL0, bitYL1;
int inByte='0';
int heatSetting=0;
boolean cuttingInProgress = false;
int aspXL, aspXR, aspYL, aspYR;   // Anzahl der clocks, die jeweils übersprungen werden, bis zum nächsten Schritt
int countLeft=0, countRight=0;  // Anzahl der X-Schritte auf der jeweiligen Seite des aktuellen Vektors
unsigned long masterClock;
unsigned long clockXL, clockXR, clockYL, clockYR;  // Schrittzähler für die jeweiligen Achsen
int xrSign, xlSign, yrSign, ylSign;  // Richtungen (werden von requestVector erzeugt)
boolean validVectorL, validVectorR;    // Erkennung des Endes der Vektor-Daten
char xySelectLeft, xySelectRight;    // Neu seit 1.4.0: Übergabe der Refernzachse

// ============= Initialisierung ==================================
void setup() {                
  // initialize the digital pins as outputs.
  // X-Links
  pinMode(8, OUTPUT);   
  pinMode(9, OUTPUT); 
  
  // Y-Links
  pinMode(10, OUTPUT);   
  pinMode(11, OUTPUT);
  
  // X-Rechts
  pinMode(A0, OUTPUT);   
  pinMode(A1, OUTPUT);
  
  // Y-Rechts
  pinMode(A2, OUTPUT);   
  pinMode(A3, OUTPUT);   
  
  // Heizung für den Draht (PWM Output)
  pinMode(3, OUTPUT);
  analogWrite(3,0);   // Heizung aus
  
  //Initialisiere digitale Inputs für Endschalter
  pinMode(12, INPUT);   // X-Rechts
  pinMode(13, INPUT);   // Y-Rechts
  pinMode(A4, INPUT);   // X-Links
  pinMode(A5, INPUT);   // Y-Links
  
  // initialize serial IO
  Serial.begin(9600);
  
  masterClock = 0;
}


// ====================== Hauptschleife ==========================
void loop() {

  if (cuttingInProgress) {  // Maschine im Schneidemodus - Vektoren werden angefordert

     if (countLeft==0) validVectorL = requestVector('L');
     if (countRight==0) validVectorR = requestVector('R');
     
     if (validVectorL || validVectorR) {     // Erst wenn beide Seiten keine validen Vektoren mehr haben, wird beendet
        xlDir=0; xrDir=0; ylDir=0; yrDir=0;
        if (masterClock>clockXL) {
            xlDir=xlSign; 
            clockXL+=aspXL;   // das ist die zentrale Stelle, die sicherstellt, dass alle <aspXL> Durchläufe
                              // ein Schritt ausgeführt wird. Es wird einfach immer weitergezählt, da hier die 
                              // Richtung keine Rolle spielt. Alle anderen Achsen entsprechend...
            if (xySelectLeft == 'X') countLeft--;      // Wenn es mehr X- als Y-Schritte gibt, dann werden diese heruntergezählt.
        }         
        if (masterClock>clockXR) {
            xrDir=xrSign; 
            clockXR+=aspXR; 
            if (xySelectRight == 'X') countRight--;
        }         
        if (masterClock>clockYL) {
            ylDir=ylSign; 
            clockYL+=aspYL; 
            if (xySelectLeft == 'Y') countLeft--;// Wenn es mehr Y- als X-Schritte gibt, dann werden diese heruntergezählt.
        }         
        if (masterClock>clockYR) {
            yrDir=yrSign; 
            clockYR+=aspYR;
            if (xySelectRight == 'Y') countRight--; 
        }         
     
        oneStep(xrDir, yrDir, xlDir, ylDir);
        masterClock++;
        delayMicroseconds(200);   // Seit 1.4.0 werden mehr Schritte übergeben, für bessere Präzission. Daher kürzerer Delay (vorher 500us)
        
     } else {
        cuttingInProgress=false;   // wenn keine gültiger Vector mehr vorhanden dann umschalten in manuell
        xlDir=0; xrDir=0; ylDir=0; yrDir=0;
        delay(1500);
        analogWrite(3,0); // Heizung ausschalten
     } 
  } 
  else
  {   // Maschine im manuellen Modus - alle Tasteneingaben werden abgefragt.     
    if (Serial.available() > 0) {
       inByte = Serial.read();  // get incoming byte
       if (inByte=='1') yrDir=-1; else 
       if (inByte=='2') yrDir=1; else
       if (inByte=='3') xrDir=-1; else 
       if (inByte=='4') xrDir=1; else     
       if (inByte=='5') ylDir=-1; else 
       if (inByte=='6') ylDir=1; else     
       if (inByte=='7') xlDir=-1; else 
       if (inByte=='8') xlDir=1; else
       if (inByte=='0') {yrDir=0; xrDir=0; xlDir=0; ylDir=0; analogWrite(3,0);}
       if (inByte=='H') readSetHeat();
       if ((inByte=='D')||(inByte=='U')||(inByte=='B')||(inByte=='F')) readMove(inByte);
       if (inByte=='S') {
           cuttingInProgress = true;   // hiermit schaltet der Maschine um zum schneiden.
           validVectorL = true;
           validVectorR = true;
           countLeft=0;    // So wird sichergestellt, dass beim Umschalten in den Schneidemodus
           countRight=0;   // Auf jeden Fall die Counters auf Null stehen und somit der erste vektor angefordert wird.
           clockXL=0; clockXR=0; clockYL=0; clockYR=0;
           masterClock=0;
       }
     }  
     oneStep(xrDir, yrDir, xlDir, ylDir);
     if (digitalRead(12)==LOW && xrDir==1) {xrDir=0; Serial.print('A'); delay(20);} 
     if (digitalRead(13)==LOW && yrDir==1) {yrDir=0; Serial.print('a'); delay(20);}    
     if (digitalRead(A4)==LOW && xlDir==1) {xlDir=0; Serial.print('B'); delay(20);}    
     if (digitalRead(A5)==LOW && ylDir==1) {ylDir=0; Serial.print('b'); delay(20);}    
     delay(10);              // wait 20ms
   }   
}

// Setting für eingestellten Heizstrom lesen und PWM entsprechend programmieren
// Format auf der seiellen Schnittstelle: Hxx    xx = 2-Byte Hex-Zahl
void readSetHeat() {
    int hexNum;
  
    heatSetting = readNumSerial(2);   // 2 byte lesen
    analogWrite(3,heatSetting);
}    

// Rechte und linke Seite um <steps> Schritte verfahren
// dir: U = Up, D = Down, B = Back, F = Forward
void readMove(char dir) {
  int i, steps, hexNum, digit;
  
  steps = readNumSerial(4);
  
  for (i=0; i<steps; i++) {
    if (dir=='U') oneStep(0,-1,0,-1);
    if (dir=='D') oneStep(0,1,0,1);
    if (dir=='B') oneStep(1,0,1,0);
    if (dir=='F') oneStep(-1,0,-1,0);
    delay(10);
  }
}  
    
boolean requestVector(char side) {
   char stpSign, aspXSign, aspYSign;
   int i;
   int hexNum, steps=0, aspX=0, aspY=0, digit;
   boolean exitVal;
   char select, xySelect;
   String text;
   
   Serial.print(side);    // sende request
   exitVal=true;

   while (Serial.available() == 0) {}   // Neu ab 1.4.0: Es wird die Referenzachse mitgegeben
   select=Serial.read();               
   if (select=='#') exitVal=false;   // letzter Vektor gelesen - beenden; 
   else xySelect = select;   // Wichtig es darf kein # in xySelect stehen, weil sonst die Counter nicht mehr heruntergezählt werden
 
   while (Serial.available() == 0) {}   // lese Vorzeichen der Schritte
   stpSign=Serial.read();               
   
   steps = readNumSerial(4); 

   while (Serial.available() == 0) {}   // lese Vorzeichen von aspX
   aspXSign=Serial.read();               

   aspX = readNumSerial(4); 

   while (Serial.available() == 0) {}   // lese Vorzeichen von aspY
   aspYSign=Serial.read();               

   aspY = readNumSerial(4); 
  
   if (side=='L') {
     if (aspXSign=='+') xlSign=-1; else xlSign=1;
     if (aspYSign=='+') ylSign=-1; else ylSign=1;
     aspXL = aspX;
     aspYL = aspY;
     countLeft = steps;
     xySelectLeft = xySelect;
   }  
   if (side=='R') {
     if (aspXSign=='+') xrSign=-1; else xrSign=1;
     if (aspYSign=='+') yrSign=-1; else yrSign=1;
     aspXR = aspX;
     aspYR = aspY;
     countRight = steps;
     xySelectRight = xySelect;
   }  
   
   Serial.println(aspX);
//   if (exitVal) Serial.println(masterClock); else Serial.println("-ende-");    // nur für debug
   
   return(exitVal);
}

// Liest eine HexZahl von der Seriellen Schnittstelle
// numberofBytes gibt die Anzahl der HexCharacters an, die erwartet werden
// Der ReturnWert der Funktion enthält die gelesene Zahl. 
long readNumSerial(int numberOfBytes) {
  int i, digit, hexNum, returnValue;
  
  digit=1; returnValue=0;
  for (i=0; i<numberOfBytes-1; i++) digit*=16;
  for (i=0; i<numberOfBytes; i++) {     
     while (Serial.available() == 0) {}
     hexNum = unHex(Serial.read());
     returnValue += digit*hexNum;
     digit=digit/16;
  }   
  return(returnValue);
}  

int unHex(char hexChar) {
  int (outByte);
  
  if (hexChar>='A') outByte=hexChar-'A'+10; else outByte=hexChar-'0';
  return(outByte);
}

// oneStep führt einen Schritt auf einer oder mehreren Achsen aus
// -1 rückwärts
// 1  vorwärts
// 0  kein Schritt
void oneStep(int xr, int yr, int xl, int yl) {
  boolean b0xr, b0yr, b0xl, b0yl;   // alten Zustand zwischenspeichern

  b0xr=bitXR0;   // alten Zustand zwischenspeichern
  b0yr=bitYR0;
  b0xl=bitXL0;
  b0yl=bitYL0;

  if (xr==1) {bitXR0 = !bitXR1; bitXR1 = b0xr;};
  if (xr==-1) {bitXR0 = bitXR1; bitXR1 = !b0xr;};
  if (xl==1) {bitXL0 = !bitXL1; bitXL1 = b0xl;};
  if (xl==-1) {bitXL0 = bitXL1; bitXL1 = !b0xl;};
  if (yr==1) {bitYR0 = !bitYR1; bitYR1 = b0yr;}; 
  if (yr==-1) {bitYR0 = bitYR1; bitYR1 = !b0yr;};
  if (yl==1) {bitYL0 = !bitYL1; bitYL1 = b0yl;};
  if (yl==-1) {bitYL0 = bitYL1; bitYL1 = !b0yl;};
  
  digitalWrite(8, bitXL1);   
  digitalWrite(9, bitXL0);   
  digitalWrite(10, bitYL0);    // Hier sind die bits auf der Platine vertauscht   
  digitalWrite(11, bitYL1);   
  digitalWrite(A0, bitXR1);   
  digitalWrite(A1, bitXR0);
  digitalWrite(A2, bitYR1);   
  digitalWrite(A3, bitYR0);
}  

