sketch_87_Segway_BalanceTest

sketch_87_Segway_BalanceTest
  /*Der Neigungswinkel wird dem Sensor-Modul MPU-6050 gemessen, das ueber I2C-Schnittstelle an den ESP verbunden ist.
  Der Takt des I2C-Bus ist erhoeht von standardmaessig 100kHz auf 400kHz, mit Wire.setClock(400000).
  
  In die Variable "BALANCEWINKEL" wird der Neigungswinkel eingetragen, bei dem der Segway nah an der Balance steht.
  Sonst bewegt sich der Segway in eine Richtung auch ohne Fahrbefehl.
  Er kann mit sketch_83_MPU6050_tockn_test ermittelt werden.
  Das Bibliotheksprogramm MPU6050_tockn.h muss aus dem Bibliotheksmanager geladen werden.
  Bei der gewaehlten Anordung ist der Winkel X relevant, dazu passt der Befehl unten: WinkelMPU = mpu6050.getAngleX()
  
  Der Segway wird etwa in Balance-Lage gehalten. Wenn das USB-Kabel angeschlossen ist, erscheint nach Hochladen die Ausschrift: 
  Calculating gyro offsets
  DO NOT MOVE MPU6050...
  Done!
  X : -100.00
  Y : -1.36
  Z : 1.42
  Program will start after 3 seconds
  ========================================-100.00   (Winkel X ist nahe des Balancewinkels)
  
  Das Schrittmotor-Treiber-Modul mit A4988 ist auf 1/8 Schritt-Betrieb eingestellt:
  Eingaenge MS1 und MS2 an +5V sowie MS3 an GND. 
  Diese Verbindungen sollten geprueft werden, sonst unerklaerliche Fehlfunktion.
  Am Potentiometer Einstellung Phasenstrom experimentell so, dass Funktion erfuellt wird, aber nicht zu hoch wegen Stromverbrauch.
  
  Akku ausreichend geladen ? Siehe Voltmeter Anzeige am Segway.
  Min 8V notwendig fuer die A4988 und das bei Betrieb der Motoren. 
  Max 9V zulaessig an VIN des ESP (Einstellung 6...7V am Modul LM2596S).*/
  
  /* Pins am ESP8266 fuer Verbindung zu den Schrittmotor-Treibern: */
  #define MotLinksD D6                                               // Drehrichtung Motor links von Pin D6           
  #define MotLinksS D5                                               // Schritt Motor links von Pin D5
  #define MotRechtsD D0                                              // Drehrichtung Motor rechts von Pin D0
  #define MotRechtsS D3                                              // Schritt Motor rechts von Pin D3
  #define MotEN D4                                                   // EN/ beide Motoren, aktiviert wenn LOW  
  
  /* Vorbereiten Kommunikation mit Gyrosensor MPU-6050: */
  #include "Wire.h"                                                  // Bibliotheksprogramm fuer I2C-Verbindung (zu MPU-6050)
  #include "MPU6050_tockn.h"                                         // Bibliotheksprogramm fuer Funktion des MPU-6050
  MPU6050 mpu6050(Wire);
   
  /* Einige Parameter festlegen und globale Variablen definieren: */
  #define BALANCEWINKEL -100.0                                       // Neigungswinkel, welcher nah an der Balance liegt, haengt von der Anordnung ab     
  #define RICHTUNGAEND 200                                           // Wie schnell soll der Segway die Fahrtrichtung aendern 
  #define GESCHWIND 200.0                                            // Mit diesem Inkrement wird die Geschwindigkeit veraendert                                                                     
  #define pidMAX 6000.0                                              // Maximalwert fuer die Stellgroesse am Reglerausgang (pid) und damit Schrittfrequenz... 
                                                                     // ...1000000/6000= 166 MicrosProSchritt (1/8 Schritt=0,225 Grad) entspr. 1350 Grad/Sek = 3,75 Rad-Umdr/Sek
  #define iMAX 6000.0                                                // zusaetzlich auch den i-Anteil begrenzen                                                                                                                                                                                                                                           
  #define LOOPZEIT 8                                                 // Zeitabstand zwischen zwei Messungen am MPU6050 + Neuberechnung der Stellgroesse...
                                                                     // ... dabei aber keine Ausfuehrung von Schritten - optimale Loopzeit austesten
  int MotLinksGeschwind = 0;                                         // Geschwindigkeit in 1/8 Schritte a 0,225 Grad/Sek , Initialwert 0, wird dann...
  int MotRechtsGeschwind = 0;                                        // ...durch Stellgroesse pid plus Vorgabe einer Drehung (RICHTUNGAEND) veraendert   
  long Looptimer = 0;                                                // In dieser Variablen wird die Zeit gespeichert, die mit millis vgl wird                                                                                                                                                                          
  float Abweich = 0;                                                 
  float LetzteAbweich = 0;                                           // Wird benoetigt fuer den D-Anteil der Regelung                                
  float BalanceWinkel;                                               // Beim Start wird vorerst dieser Wert als Balancewinkel verwendet (siehe Ende vom setup)
  float WinkelMPU;                                                   // Aktueller Neigungswinkel gemaess Messung durch MPU-6050
  float p=0.0;                                                       // Stellgroesse Proportional-Anteil, am Ausgang vom PID-Regler
  float i=0.0;                                                       // Stellgroesse Integral-Anteil, am Ausgang vom PID-Regler
  float d=0.0;                                                       // Stellgroesse Differenzial-Anteil, am Ausgang vom PID-Regler
  float pid=0.0;                                                     // Gesamt-Stellgroesse, am Ausgang vom PID-Regler             
  
  /* Festlegung Parameter fuer PID-Neigungsregelung (experimentell ermitteln):*/
  int kp= 350;                                                       // P-Anteil des Reglers                                  
  int ki= 50;                                                        // I-Anteil des Reglers      
  int kd= 00;                                                        // D-Anteil des Reglers                                                                                                                               
  
  void setup()
  {
   Serial.begin(115200);                                             // Baudrate fuer Serial Monitor hoch setzen (Programmlaufzeit)
   
   pinMode(MotLinksD, OUTPUT);
   pinMode(MotLinksS, OUTPUT);
   pinMode(MotRechtsD, OUTPUT);
   pinMode(MotRechtsS, OUTPUT);  
   pinMode(MotEN, OUTPUT); 
      
   digitalWrite(MotEN,LOW);                                          // Motortreiber sofort aktivieren                                                                   
   delay(1000);                                                      // Etwas warten, bis eventuelle Erschuetterungen abgeklungen sind              
   Wire.begin(D2,D1);                                                // Initialisierung I2C Bus fuer Kommunikation mit MPU6050(ESP als Master)
   Wire.setClock(400000);                                            // I2C Takt erhoehen von standardmaessig 100kHz auf 400kHz
   mpu6050.begin();                                                  // Bibliotheksprogramm "mpu6050" startet, Segway nahe Balance-Winkel halten
   mpu6050.calcGyroOffsets(true);                                    // Waehrend der Kalibrierung nicht bewegen! 
   mpu6050.update();                                                 // MPU-6050 auslesen
   WinkelMPU = mpu6050.getAngleX();                                                                   
   Serial.println(WinkelMPU);                                        // Ausgabe des aktuellen Neigungswinkels am Seriellen Monitor
   BalanceWinkel = WinkelMPU;                                        // Wenn beim Start der Segway nicht genau auf BALANCEWINKEL steht, wird die Regelabweichung...   
  }                                                                  // ...sofort hoch, die Raeder drehen schnell. Der Wert wird vorerst dem tatsaechl Winkel angepasst  
  
  
  void loop()
  { 
   if(millis() > Looptimer)                                           // Der folgende Programmabschnitt nur erst wieder wenn Loopzeit vorbei ist
   {                                                             
    Looptimer = millis() + LOOPZEIT;                                  // Looptimer sofort wieder aktualisieren  
    mpu6050.update();                                           
    WinkelMPU = mpu6050.getAngleX();                             
    //Serial.println(WinkelMPU);                                      // Ausgabe des Neigungswinkels am Seriellen Monitor, deaktiviert wegen Programmlaufzeit                                                             
    if (BalanceWinkel < BALANCEWINKEL) BalanceWinkel +=0.1;           // Allmaehliche Angleichung des BalanceWinkel an den BALANCEWINKEL (fuer Start, siehe oben)
    if (BalanceWinkel > BALANCEWINKEL) BalanceWinkel -=0.1;  
             
    /* Berechnung des PID-Reglers (Neigungsregelung des Segway fuer Erhalt der Balance):*/  
         
    Abweich = WinkelMPU - BalanceWinkel;                              // Regelabweichung bestimmen     
    p = kp  *  Abweich;                                               // Proportional-Anteil berechnen
    i = i+(ki * Abweich);                                             // Integral-Anteil berechnen 
    if(i < -iMAX) i= -iMAX;
    if(i > iMAX) i= iMAX;                                                                                                           
    d = kd * (Abweich-LetzteAbweich);                                 // Differenzial-Anteil berechnen 
    LetzteAbweich = Abweich;                       
    pid = p + i + d;
    
    // Stellgroesse fuer MotLinksGeschwind und MotRechtsGeschwind berechnen
    if(pid < 50 && pid > -50)pid = 0;                                 // Bei geringer Abweichung von der Balance eine "Totzone"     
    if(abs(pid) < pidMAX)                                             // Falls pid ueber pidMAX gestiegen sein sollte, bleiben die Werte unveraendert
    {                                                               
     MotLinksGeschwind = pid;                                                         
     MotRechtsGeschwind = pid;                       
    }                                                               
    if(WinkelMPU < -140 || WinkelMPU > -60) digitalWrite(MotEN,HIGH); // Wenn Schraeglage zu gross - Motoren abschalten                                                                                  
   }
   MotLinks();                                                        // Aufruf der Unterprogramme zur Ansteuerung der Motoren
   MotRechts();
  }
  
  
  void MotLinks()
  {
   if(MotLinksGeschwind == 0) return;                                 // Wenn Null - zurueck zu Beginn von void loop, sonst:   
   digitalWrite(MotLinksS, LOW);                                      // A4988-Eingang STEP auf LOW setzen, notwendig noch delay...                                         
   delayMicroseconds(3);                                              // ...weil der A4988-Eingang STEP eine min Impulslaenge 1 Mikrosek verlangt                                                                                                                                                           
   static long LetzterSchritt = 0;                                    // Variable mit "static": Wert 0 wird nur beim ersten Durchlauf zugewiesen                                                                              
   long MicrosProSchritt = 1000000 / abs(MotLinksGeschwind);          // Berechn der Zeit zwischen zwei Schritten (1/8 Schritten 0,225 Grad)                                                                                                                                 
   digitalWrite(MotLinksD, MotLinksGeschwind > 0 ? HIGH : LOW);       // Richtungsangabe setzen, Ausgabe an Pin Motor1D
                                                                      // Ergebnis ist LOW, wenn Motor1Geschwind > 0 ist. Sonst HIGH.
   if(micros() - LetzterSchritt > MicrosProSchritt)
   {                                                                  // Wenn die Zeit ueberschritten ist erfolgt ein Schritt
    digitalWrite(MotLinksS, HIGH);                                    // A4988-Eingang STEP auf HIGH setzen, ein 1/8 Schritt 0,225 Grad wird ausgefuehrt 
    LetzterSchritt = micros();                                        // Variable wird wieder aktualisiert, fuer erneuten Vgl mit micros                                                         
   }
  }
  
  void MotRechts()
  {
   if(MotRechtsGeschwind == 0) return;                                // Wenn Null - zurueck zu Beginn von void loop, sonst:   
   digitalWrite(MotRechtsS, LOW);                                     // A4988-Eingang STEP auf LOW setzen, notwendig noch delay...                                         
   delayMicroseconds(3);                                              // ...weil der A4988-Eingang STEP eine min Impulslaenge 1 Mikrosek verlangt                                                                                                                                                           
   static long LetzterSchritt = 0;                                    // Variable mit "static": Wert 0 wird nur beim ersten Durchlauf zugewiesen                                                                              
   long MicrosProSchritt = 1000000 / abs(MotRechtsGeschwind);         // Berechn der Zeit zwischen zwei Schritten (1/8 Schritten 0,225 Grad)                                                                                                                                 
   digitalWrite(MotRechtsD, MotRechtsGeschwind > 0 ? LOW : HIGH);     // Anders als bei void MotLinks hier HIGH:LOW, da Drehrichtung invers sein muss...
                                                                      // ......weil die Achse in entgegensetzte Richtung zeigt
   if(micros() - LetzterSchritt > MicrosProSchritt)
   {                                                                  // Wenn die Zeit ueberschritten ist erfolgt ein Schritt
    digitalWrite(MotRechtsS, HIGH);                                   // A4988-Eingang STEP auf HIGH setzen, ein 1/8 Schritt 0,225 Grad wird ausgefuehrt 
    LetzterSchritt = micros();                                        // Variable wird wieder aktualisiert, fuer erneuten Vgl mit micros                                                         
   }
  }