/*Es wird Arduino Nano auf I/O Erweiterungs Shield Modul verwendet.
  Die Spannungsversorgung der Servos geht direkt von 5V oder von 3,3V (drehen langsamer) des Nano.
  Nur moeglich bei Micro Servos SG90 und wenn immer nur ein Servo angesteuert wird (die anderen stromlos).
  Es wird immer nur 1 Servo des Roboters bewegt, die anderen bleiben stromlos.
  Die Bewegungen werden ueber Eingaben im seriellen Monitor aktiviert.
  Zum Beispiel Eingabe "d1500" bewegt den Servo Drehung in die Mittelstellung, "g600" den Servo Greifer in
  eine Endstellung.
  
  PWM-ON Zeit fuer Mittelstellung theoretisch 1,5ms kann aber abweichen (experimentell ermitteln).
  Impulslaenge Schwenkbereich -90 Grad...+90 Grad theoretisch 1...2ms, aber bei SG90 praktisch etwa 0,6ms...2,6ms. 
  Die "for"-Schleife sorgt dafuer, dass der Servo so lange aktiviert bleibt bis er seine Zielstellung erreicht hat.
  Danach haelt er die Position nicht mehr, man kann von Hand verdrehen. 
  Bei 4,8V braucht der Servo SG90 etwa 300ms fuer Drehung um 60 Grad.
  */
  #include "Drehung.h"
  #include "Greifer.h"
  #include "Heben.h"
  #include "VorZurueck.h"
  
  void setup()
  { 
   pinMode(2, OUTPUT);                    //Nano digital Pin D2 (Ausgang zu Servo Drehung)    
   pinMode(4, OUTPUT);                    //Nano digital Pin D4 (Ausgang zu Servo Greifer)
   pinMode(6, OUTPUT);                    //Nano digital Pin D6 (Ausgang zu Servo Heben)                
   pinMode(8, OUTPUT);                    //Nano digital Pin D8 (Ausgang zu Servo VorZurueck)
   Serial.begin(9600);                    //Initialisierung Serial Monitor  (beachte: muss auf 9600 eingestellt sein)
   Serial.println("Bereit");                     
  }
  
  void loop() 
  {
   if (Serial.available() > 0)            // Wenn Daten vom Serial Monitor da sind (d.h.eingegeben worden sind)...
   { 
    int inByte = Serial.read();           // ...dann lies das erste Byte und speichere es in der Variable inByte
    switch (inByte)                       // und nimm den Wert, der uebertragen wurde, genauer unter die Lupe.
    {           
     case 'd':                            // wenn dieser das Zeichen (character) 'd' fuer Drehen ist...
     {
      int PWM_Dr = Serial.parseInt();     // dann lies erstmal eine Zahl ein (wenn irgendetwas anders kam, ist das Ergebnis 0)
      Serial.print ("Drehen: ");
      Serial.println (PWM_Dr);
      Drehung(PWM_Dr);
      break;                              // hoere hier auf.
     }
     case 'g':                            // wenn dieser das Zeichen (character) 'g' fuer Greifer ist...
     {
      int PWM_Gr =Serial.parseInt();      // dann lies erstmal eine Zahl ein (wenn irgendetwas anders kam, ist das Ergebnis 0)
      Serial.print ("Greifer: ");
      Serial.println (PWM_Gr);
      Greifer(PWM_Gr);
      break;                              // hoere hier auf.
     }
      case 'h':                           // wenn dieser das Zeichen (character) 'g' für Heben ist...
     {
      int PWM_He =Serial.parseInt();      // dann lies erstmal eine Zahl ein (wenn irgendetwas anders kam, ist das Ergebnis 0)
      Serial.print ("Heben: ");
      Serial.println (PWM_He);
      Heben(PWM_He);
      break;                              // hoere hier auf.
     }
     case 'v':                            // wenn dieser das Zeichen (character) 'v' fuer VorZurueck ist...
     {
      int PWM_Vo =Serial.parseInt();      // dann lies erstmal eine Zahl ein (wenn irgendetwas anders kam, ist das Ergebnis 0 )
      Serial.print ("VorZurueck: ");
      Serial.println (PWM_Vo);
      VorZurueck(PWM_Vo);
      break;                              // hoere hier auf.
      }
     default:                             // bei unbekannten Kommandos machen wir einfach garnichts...
     break; 
    }
   }
  }

  void Drehung(int PWM_Dr)             //vor "PWM_Dr" muss nochmal "int" (aber nicht im Hauptprogramm)
  {
   for(byte i=0; i<=60 ; i++)          //Wird i-mal wiederholt, der Servo hat i x 20ms Zeit seine Position zu erreichen
   {
    digitalWrite(2,HIGH);
    delayMicroseconds(PWM_Dr);         //Zeitvorgabe High-Impuls
    digitalWrite(2,LOW);
    delay(19);                         //Ergaenzung auf Zykluszeit etwa 20ms (delayMicroseconds funktioniert nur bis 16383 "integer") 
   }
  }

  void Greifer(int PWM_Gr)             //vor "PWM_Gr" muss nochmal "int" (aber nicht im Hauptprogramm)
  {
   for(byte i=0; i<=60 ; i++)          //Wird i-mal wiederholt, der Servo hat i x 20ms Zeit seine Position zu erreichen
   {
    digitalWrite(4,HIGH);
    delayMicroseconds(PWM_Gr);         //Zeitvorgabe High-Impuls
    digitalWrite(4,LOW);
    delay(19);                         //Ergaenzung auf Zykluszeit etwa 20ms (delayMicroseconds funktioniert nur bis 16383 "integer") 
   }
  }

  void Heben(int PWM_He)               //vor "PWM_He" muss nochmal "int" (aber nicht im Hauptprogramm)
  {
   for(byte i=0; i<=60 ; i++)          //Wird i-mal wiederholt, der Servo hat i x 20ms Zeit seine Position zu erreichen
   {
    digitalWrite(6,HIGH);
    delayMicroseconds(PWM_He);         //Zeitvorgabe High-Impuls
    digitalWrite(6,LOW);
    delay(19);                         //Ergaenzung auf Zykluszeit etwa 20ms (delayMicroseconds funktioniert nur bis 16383 "integer") 
   }
  }

  void VorZurueck(int PWM_Vo)          //vor "PWM_Vo" muss nochmal "int" (aber nicht im Hauptprogramm)
  {
   for(byte i=0; i<=60 ; i++)          //Wird i-mal wiederholt, der Servo hat i x 20ms Zeit seine Position zu erreichen
   {
    digitalWrite(8,HIGH);
    delayMicroseconds(PWM_Vo);         //Zeitvorgabe High-Impuls
    digitalWrite(8,LOW);
    delay(19);                         //Ergaenzung auf Zykluszeit etwa 20ms (delayMicroseconds funktioniert nur bis 16383 "integer") 
   }
  }