/***************************************************************
 ArduinoMill
 
 CNC Steuerung mit Arduino    CC-BY-NC-SA  Ralf Stoffels 12/2014

 Version: 0.9   Urversion zum Ausprobieren der Basics
 
 ***************************************************************/

// Konstanten
#define X_HOME  A2
#define Y_HOME  A4

//Globale Variablen 
boolean bitx1, bitx2;  // Zustand x-Motor
boolean bity1, bity2;  // Zustand y-Motor
boolean bitz1, bitz2;  // Zustand z-Motor

int xDir = 0;    // Fahrtrichtung der Motoren
int yDir = 0;
int zDir = 0;
boolean millState = 0; // Zustand des Fräserrelais
long zSteps;    // Wird benötigt, um die Z-Achse um einen definierten Wert zu verfahren. 
long stepDelay = 0;   // Delay, mit dem ie Fahrgeschwindigkeit verlangsamt werden kann (0..255);
int aspX, aspY;   // Verhältnis der Schritte auf x- und y-Achse
int count = 0;   // Anzahl der Schritte der Referenzachse
char xySelect;  // Gibt an, welche die Referenzachse ist
int xSign, ySign;  // Fahrtrichtung der jeweiligen Achse 
unsigned long masterClock = 0;
unsigned long clockX = 0, clockY = 0;
boolean validVector;  // es wurde ein Fräsvektor empfangen

//Wir beim Start des Arduinos ausgeführt
void setup() {      
  // X-Motor Outputs
  pinMode(6, OUTPUT);     
  pinMode(7, OUTPUT);     
  pinMode(8, OUTPUT);     
  pinMode(9, OUTPUT);     
  
  // Y-Motor Outputs
  pinMode(2, OUTPUT);     
  pinMode(3, OUTPUT);     
  pinMode(4, OUTPUT);     
  pinMode(5, OUTPUT);     

  // Z-Motor Outputs
  pinMode(13, OUTPUT);     
  pinMode(12, OUTPUT);     
  pinMode(11, OUTPUT);     
  pinMode(10, OUTPUT);     
 
  // Endschalter 
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);

  // Fräserrelais
  pinMode(A0, OUTPUT);
  digitalWrite(A0, LOW);  // Fräse auf jeden Fall ausschalten!!!
  
  bitx1=0;  // X-Motor abschalten
  bitx2=0;

  bity1=0;  // Y-Motor abschalten
  bity2=0;

  bitz1=0;  // Z-Motor abschalten
  bitz2=0;

  zSteps = -1;  // Das Verfahren um eine feste z-Distanz ist inaktiv 
  stepDelay = 0;

  StepperOn();

  count = 0;
  masterClock = 0;
  clockX = 0;
  clockY = 0;
  validVector = false;
  
  // initialize serial IO
  Serial.begin(9600);
}


// Läuft kontinuierlich
void loop() {
  int inByte;
  
  if (Serial.available() > 0) {
     inByte = Serial.read();  // get incoming byte
     if (inByte == 'X') { if(xDir != 0) xDir = 0; else xDir = 1; }
     if (inByte == 'x') { if(xDir != 0) xDir = 0; else xDir = -1; }
     if (inByte == 'Y') { if(yDir != 0) yDir = 0; else yDir = 1; }
     if (inByte == 'y') { if(yDir != 0) yDir = 0; else yDir = -1; }
     if (inByte == 'Z') { if(zDir != 0) zDir = 0; else zDir = 1; }
     if (inByte == 'z') { if(zDir != 0) zDir = 0; else zDir = -1; }
    
     if (inByte == 'F') digitalWrite(A0, HIGH);   //Fräserrelais ein
     if (inByte == 'f') digitalWrite(A0, LOW);    // Fräserrelais aus
     
     if (inByte == '-') { zSteps = readNumSerial(4); zDir = -1; }
     if (inByte == '+') { zSteps = readNumSerial(4); zDir = 1; }
     
     if (inByte == '%') stepDelay = readNumSerial(2); 
   
     if (inByte == '!') validVector = receiveVector();  // Hiermit wird ein neuer Vektor übermittelt 
  }
  

  if (validVector) {     // Wenn ein Fräsvektor empfangen wurde, dann werden die x-/y-Achsen gefahren
     xDir = 0; yDir = 0;
     if (masterClock>clockX) {
         xDir = xSign; 
         clockX += aspX; // das ist die zentrale Stelle, die sicherstellt, dass alle <aspX> Durchläufe
                         // ein Schritt ausgeführt wird. Es wird einfach immer weitergezählt, da hier die 
                         // Richtung keine Rolle spielt. Y-Achse entsprechend...
         if (xySelect == 'X') count--;      // Wenn es mehr X- als Y-Schritte gibt, dann werden diese heruntergezählt.
     }         
     if (masterClock>clockY) {
        yDir = ySign; 
        clockY += aspY; 
        if (xySelect == 'Y') count--;// Wenn es mehr Y- als X-Schritte gibt, dann werden diese heruntergezählt.
     }
     if (count==0) {
       Serial.write('?');  // Signalisieren, dass neuer Vektor gesendet werden kann
       validVector = false;   // zurücksetzen
       xDir = 0; yDir = 0;    // Und zum Schluss die Motoren stoppen, falls sie noch laufen
     }
     masterClock++;
  } 
     
  oneStep(xDir,yDir,zDir);
  
  if (zSteps > 0) zSteps--;  // Nur wenn zSteps übermittelt wurde, wird dekrementiert (Normalerweise ist zSteps = -1)
  if (zSteps == 0) {
     zDir = 0; 
     zSteps = -1;
     Serial.write('?');
   }  // Zielpunkt erreicht - zSteps auf Default zurücksetzen und melden, dass ein neuer Befehl empfangen werden kann

  if ((digitalRead(X_HOME) == LOW) && (xDir == -1)) xDir = 0;
  if ((digitalRead(Y_HOME) == LOW) && (yDir == -1)) yDir = 0;
  if (validVector) delayMicroseconds(20+stepDelay); else delayMicroseconds(4000);
}

// Schaltet die Motoren aktiv (noch keine Bewegung)
void StepperOn() {    // Schaltet die Stepper in den aktiven Zustand
  bitx1=0;
  bitx2=1;
  bity1=0;
  bity2=1;
  bitz1=0;
  bitz2=1;
}

// oneStep führt einen Schritt auf einer oder mehreren Achsen aus
// -1 rückwärts
// 1  vorwärts
// 0  kein Schritt
void oneStep(int xDir, int yDir, int zDir) {
  boolean bx1,by1,bz1;
  
  bx1 = bitx1;   // Alten Zustand sichern
  by1 = bity1;  
  bz1 = bitz1;

  if (xDir == 1)  { bitx1 = !bitx2; bitx2 = bx1; }  // x-Motor
  if (xDir == -1) { bitx1 = bitx2;  bitx2 = !bx1; }
  
  if (yDir == -1)  { bity1 = !bity2; bity2 = by1; }  // y-Motor
  if (yDir == 1) { bity1 = bity2;  bity2 = !by1; }
  
  if (zDir == -1)  { bitz1 = !bitz2; bitz2 = bz1; }  // z-Motor
  if (zDir == 1) { bitz1 = bitz2;  bitz2 = !bz1; }

  digitalWrite(8, bitx1);  // x-Motor outputs setzen
  digitalWrite(9, !bitx1);
  digitalWrite(7, bitx2);
  digitalWrite(6, !bitx2);

  digitalWrite(4, bity1);  // y-Motor outputs setzen
  digitalWrite(5, !bity1);
  digitalWrite(3, bity2);
  digitalWrite(2, !bity2);
  
  digitalWrite(12, bitz1);  // z-Motor outputs setzen
  digitalWrite(13, !bitz1);
  digitalWrite(11, bitz2);
  digitalWrite(10, !bitz2);  

}


// Fragt den PC nach einem neuen Vektor und wartet solange, bis er gesendet wurde
boolean receiveVector() {
   char stpSign, aspXSign, aspYSign, select;
   int i;
   int hexNum, steps=0, digit;
   boolean exitVal;
   String text;
  
   exitVal=true;

   while (Serial.available() == 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 (aspXSign=='+') xSign=1; else xSign=-1;
   if (aspYSign=='+') ySign=1; else ySign=-1;
  
   count = steps;
   masterClock = 0;   // Alle Counter für diesen vektor zurücksetzen
   clockX = 0;
   clockY = 0;
   xDir = 0;
   yDir = 0;
   
//   Serial.println(count);
//   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);
}
