sketch73_esp_roboter_servos_bewegungsablauf (ESP8266 NodeMCU)

sketch73_esp_roboter_servos_bewegungsablauf
  /*In diesem Sketch wir eine Ablaufsteuerung, d.h.kompletter Bewegungsablauf, fuer den Roboter programmiert.
  Aber nur fuer die 4 Servos, nicht fuer den Schrittmotor Drehung.
  Die 2 Servos "Oben Schwenken" und "Unten Schwenken" werden staendig bestromt, da der Roboterarm sonst langsam absinkt.
  Das ist nicht moeglich mit der delay-Funktion, hier werden stattdessen die Funktionen millis() und micros()angewendet.
  Es wird jeweils 2 Sek Zeit gegeben um die neue Position sicher zu erreichen.
  Die neuen Positionen werden in die Arrays geladen. In Variable "a" muss Anzahl der Array-Elemente minus 1.
  https://starthardware.org/lektion-15-array/
  https://www.arduino.cc/reference/de/language/variables/data-types/array/
  Die Spannungsversorgung kann zwischen 5 ... 7,2V betragen (Bereich fuer die Servos).
  Wenn die Servos blockiert sind (kann vor allem beim Greifer passieren) und laenger eingeschaltet bleiben, werden sie zu heiss.
  Der Schrittmotor hat eine Nennspannung von 12V und ist besonders bei niedriger Spannung sehr schwach.
  Deshalb sollte 7,2 V verwendet werden, die Drehung nur bei angezogenen Armen erfolgen und die Servos nicht gleichzeitig betaetigt 
  werden (Spannung bricht etwas ein).
  Zur Begrenzung der Stromaufnahme sollte moeglichst nur jeweils 1 Servo betaetigt werden, d.h. nur jeweils 1 Aenderung im Array.
  
  Der Roboter muss manuell vor dem Start in die Startposition "0 Grad" gedreht werden (Referenzposition "50").
  
  Der Servo "UntenSchwenken" laesst den Roboterarm stark ueberschwingen, vor allem abwaerts. Deshalb wird dieser Servo sanfter mit 
  Zwischenpositionen bewegt. Dazu wurde eine zusaetzliche Routine eingefuegt.
  */
  
  /*Servos Position neu, in einem Array:*/  
  int ON_GR [] = {0000, 0000, 0000, 1750, 0000, 0000, 2200, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 1750, 0000 };        
  int ON_DG [] = {0000, 0000, 0000, 1000, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 2500, 2500, 2500, 0000, 1000, 0000, 0000 }; 
  int ON_OS [] = {1300, 1300, 1300, 1300, 1300, 1000, 1000, 1300, 1300, 1050, 1050, 1050, 1050, 1300, 1300, 1050, 1050, 1300 };
  int ON_US [] = {1550, 1550, 1550, 1550, 1550, 1100, 1100, 1550, 1550, 1100, 1100, 1500, 1500, 1550, 1550,  950,  950, 1550 };
  
  byte i = 0;                                 // Zaehlvariable fuer Aufruf der Array-Elemente
  byte a = 17;                                // Muss gleich der Anzahl der Array-Elemente minus 1 sein
  unsigned long timer_milli = 0;
  unsigned long timer_micro = 0;
  int US = ON_US[0];                          // Variable, in der auch Zwischenpositionen gespeichert sind (nur fuer Unten Schwenken)               
  
  void setup()
  {
   pinMode (D5, OUTPUT);                      // Servo GR  Greifer
   pinMode (D6, OUTPUT);                      // Servo DG  Drehen Greifer
   pinMode (D7, OUTPUT);                      // Servo OS  Oben Schwenken
   pinMode (D8, OUTPUT);                      // Servo US  Unten Schwenken
  }
   
  void loop()
  {
   if (micros() < ON_GR[i] + timer_micro)     // GR            
   {
    digitalWrite (D5,HIGH);          
   }
   else
   {
    digitalWrite (D5,LOW);
   }
  
   if (micros() < ON_DG[i] + timer_micro)     // DG         
   {
    digitalWrite (D6,HIGH);          
   }
   else
   {
    digitalWrite (D6,LOW);
   }
  
   if (micros() < ON_OS[i] + timer_micro)     // OS           
   {
    digitalWrite (D7,HIGH);          
   }
   else
   {
    digitalWrite (D7,LOW);
   }
  
   if (micros() < US + timer_micro)           // US (nicht ON_US[i], denn in US stehen auch Zwischenpositionen)           
   {
    digitalWrite (D8,HIGH);          
   }
   else
   {
    digitalWrite (D8,LOW);
   } 
   if (micros() > 20000 + timer_micro)        // 20ms erreicht? Dann ist ein Zyklus zum Servo beendet
   {                                           
    timer_micro = micros();
    
    int k = ON_US[i] - ON_US[i-1];            // zusaetzlicher Programmteil, nur fuer Servo Unten Schwenken
    if (k > 0)
    {
     US = US + 10;
     if (US >= ON_US[i])
     {   
      US = ON_US[i];
     }   
     else
     {
     }
    }
    else
    {
     US = US - 10;
     if (US <= ON_US[i])
     {   
      US = ON_US[i];
     }   
     else
     {
     }
    }
   } 
   else
   {
   }
   if (millis() > 2000 + timer_milli)         // 2 Sek erreicht ? Solange wird Zeit gegeben, die Position zu erreichen.
   {
    timer_milli = millis();
    if (i < a)                                // alle Elemente des Arrays abgearbeitet (kpl Bewegungsablauf erfolgt) ?
    {
     i= i + 1;
    }
    else
    {
     i = 0;                                   // erneut Beginn mit Array-Element 0 (Bewegungsablauf wiederholen)
    }
   }
   else
   {
   } 
  }