sketch73_esp_roboter_kompletter_bewegungsablauf (ESP8266 NodeMCU)

sketch73_esp_roboter_kompletter_bewegungsablauf
  /*In diesem Sketch ist eine Ablaufsteuerung, d.h.kompletter Bewegungsablauf, fuer den Roboter programmiert.
  Es werden die 4 Servos als auch der Schrittmotor (Drehung Roboter) gesteuert.
  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.
  Der Schrittmotor wird mit Schrittfrequenz 50 Schritten/s betrieben, dann kann jeweils am Ende des 20ms Zyklus fur die 
  Servos ein neuer Schritt ausgefuehrt werden (vereinfacht den Sketch).
  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 (zulaessiger 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 nicht gleichzeitig die Servos 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.
  Die Stromaufnahme des Schrittmotors ist relativ niedrig.
  
  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:*/
  //              Oberer Wuerfel                                                                                              Unterer Wuerfel
  //                                oeff  dreh  runt  zu    heb   dreh  zeig  zeig  zeig  zeig  heb   dreh  senk  oeff  heb   dreh  senk  zu    heb   dreh  zeig  zeig  zeig  zeig  heb   dreh  senk  oeff  heb   dreh
  int ON_GR [] = {0000, 0000, 0000, 1750, 0000, 0000, 2200, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 1750, 0000, 0000, 0000, 2200, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 1750, 0000, 0000, 2200 };        
  int ON_DG [] = {0000, 0000, 0000, 1000, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 2500, 2500, 2500, 0000, 1000, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 2500, 0000, 0000, 0000, 0000, 0000, 0000, 0000, 1800 }; 
  int ON_OS [] = {1300, 1300, 1300, 1300, 1300, 1000, 1000, 1300, 1300, 1050, 1050, 1050, 1050, 1300, 1300, 1150, 1150, 1300, 1300, 1100, 1100, 1300, 1300, 1050, 1050, 1050, 1050, 1300, 1300,  950,  950, 1300, 1300, 1300,};
  int ON_US [] = {1550, 1550, 1550, 1550, 1550, 1100, 1100, 1550, 1550, 1120, 1120, 1500, 1500, 1550, 1550,  950,  950, 1550, 1550,  920,  920, 1550, 1550, 1100, 1100, 1500, 1500, 1550, 1550, 1050, 1050, 1550, 1550, 1550 };
  /*Schrittmotor Drehung Position Neu:*/
  byte POS_N[] = {50,   50,   50,   50,   80,   80,   80  , 80  , 50  , 50  , 50  , 50  , 50  , 50  , 20  , 20  , 20  , 20 ,  80  , 80,   80,   80  , 50  , 50  , 50  , 50  , 50  , 50  , 20  , 20  , 20  , 20  , 50  , 50}; 
  byte i = 0;                                 // Zaehlvariable fuer Aufruf der Array-Elemente
  byte a = 33;                                // Muss gleich der Anzahl der Array-Elemente minus 1 sein
  /*fuer Schrittmotor Drehung:*/
  byte POS_A = 50;                            // Schrittmotor Position alt = Startposition "50" 
  byte Status = 1;                            // Schrittmotor Status 1...4 (PinsD3D4: HH,LL,LH,HL)
  unsigned long timer_milli = 0;
  unsigned long timer_micro = 0;
  int US = ON_US[0];                          // Variable, in der auch Zwischenpositionen gespeichert sin               
  
  
  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
   pinMode(D1, OUTPUT);                       // Schrittmotor ENA PWM Wicklung A
   pinMode(D2, OUTPUT);                       // Schrittmotor ENB PWM Wicklung B
   pinMode(D3, OUTPUT);                       // Schrittmotor Drehrichtung Wicklung A
   pinMode(D4, OUTPUT);                       // Schrittmotor Drehrichtung Wicklung B
   analogWrite (D1,1023);                     // PWM Wicklung A volle Aussteuerung - volle Spannung
   analogWrite (D2,1023);                     // PWM Wicklung A volle Aussteuerung - volle Spannung
  }
   
  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
   {                                          // und es kann ein neuer Schritt aufgerufen werden 
    timer_micro = micros();
  
    int k = ON_US[i] - ON_US[i-1];            // zusaetzlicher Programmteil, nur fuer Servo UntenSchwenken
    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
    {
    }
   }
        
    if(Status==1)                             // Hier beginnt der Programmteil zum Schrittmotor
    {
     if(POS_N [i] - POS_A > 0)
     {
      digitalWrite(D3,LOW);                   // Schrittmotor Wicklungsbestromung LH (Status 2)
      digitalWrite(D4,HIGH); 
      Status=2;
      POS_A +=1; 
     }
     else if(POS_N [i] - POS_A < 0)
     {
      digitalWrite(D3,HIGH);                  // Schrittmotor Wicklungsbestromung HL (Status 4)
      digitalWrite(D4,LOW); 
      Status=4;
      POS_A -=1; 
     }
    } 
    else if(Status == 2)
    {
     if(POS_N [i] - POS_A > 0)
     {
      digitalWrite(D3,LOW);                   // Schrittmotor Wicklungsbestromung LL (Status 3)
      digitalWrite(D4,LOW); 
      Status=3;
      POS_A +=1; 
     }
     else if(POS_N [i] - POS_A < 0)
     {
      digitalWrite(D3,HIGH);                  // Schrittmotor Wicklungsbestromung HH (Status 1)
      digitalWrite(D4,HIGH); 
      Status=1;
      POS_A -=1; 
     }
    }
    else if(Status==3)
    {
     if(POS_N [i] - POS_A > 0)
     {
      digitalWrite(D3,HIGH);                  // Schrittmotor Wicklungsbestromung HL (Status 4)
      digitalWrite(D4,LOW); 
      Status=4;
      POS_A +=1;  
     }
     else if(POS_N [i] - POS_A < 0)
     {
      digitalWrite(D3,LOW);                   // Schrittmotor Wicklungsbestromung LH(Status 2)
      digitalWrite(D4,HIGH); 
      Status=2;
      POS_A -=1;  
     }
    }
    else if(Status==4)
    {
     if(POS_N [i] - POS_A > 0)
     {
      digitalWrite(D3,HIGH);                  // Schrittmotor Wicklungsbestromung HH (Status 1)
      digitalWrite(D4,HIGH); 
      Status=1;
      POS_A +=1;  
     }
     else if(POS_N [i] - POS_A < 0)
     {
      digitalWrite(D3,LOW);                   // Schrittmotor Wicklungsbestromung LL (Status 3)
      digitalWrite(D4,LOW); 
      Status=3;
      POS_A -=1;  
     }
    } 
    if(POS_N [i] - POS_A == 0)
    {
     POS_A = POS_N [i];                       // Wenn die neue Position POS_N erreicht wurde
    }
    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
   {
   } 
  }