sketch_87_Segway_BalanceTest

sketch_87_Segway_BalanceTest
  /*Es wird getestet, ob sich der Segway in Balance haelt.
  Akku ausreichend geladen ? Min 8V notwendig fuer die A4988, bei Betrieb der Motoren. Siehe Voltmeter Anzeige am Segway. 
  Noch keine Fahrbefehle ueber Bluetooth vom Smartphone (siehe Sketch 88).
  Der Segway wird etwa in Balance-Lage gehalten. 
  Nach Hochladen kommt Ausschrift am SerialMonitor: 
  ========================================
  Calculating gyro offsets
  DO NOT MOVE MPU6050...
  Done!
  X : -1.54
  Y : 0.27
  Z : -0.65
  Program will start after 3 seconds
  ========================================
  WinkelY bei Start= -109.19   (in Grad ; wenn Segway in dieser Lage gehalten wird)
  
  Das Bibliotheksprogramm MPU6050_tockn.h muss aus dem Bibliotheksmanager geladen werden.
  Man findet den Code (.cpp und .h)auch auf der Webseite:  https://github.com/Tockn/MPU6050_tockn
  Der Balancewinkel ist abhaengig von der Anordnung des MPU-6050, wir befestigen das Modul so, dass der Winkel Y relevant ist.
  Er kann mit dem sketch_83_MPU6050_tockn_test ermittelt werden.
  In die Variable BALANCEWINKEL muss moeglichst genau dieser Wert, sonst bewegt sich der Segway in eine Richtung auch ohne Fahrbefehl.
  Takt des I2C-Bus erhoehen von standardmaessig 100kHz auf 400kHz, mit Wire.setClock(400000).
  Das Schrittmotor-Treiber-Modul mit A4988 ist auf 1/8 Schritt-Betrieb eingestellt:
  Eingaenge MS1 und MS2 an +5V sowie MS3 an GND. Eingang /RST an +5V.
  Diese Verbindungen sollten geprueft werden, sonst unerklaerliche Fehlfunktion.
  Am Potentiometer Einstellung Phasenstrom so, dass Funktion erfuellt wird.
  Beachte: Je nach Ausfuhrung des Nano evtl "ATmega328P(Old Bootloader)" einstellen*/   
  
  /* Pins am Nano fuer Verbindung zu den Schrittmotor-Treibern: */
  #define MotLinksD 5                                             // Drehrichtung Motor links von Pin D5           
  #define MotLinksS 9                                             // Schritt Motor links von Pin D9
  #define MotRechtsD 3                                            // Drehrichtung Motor rechts von Pin D3
  #define MotRechtsS 2                                            // Schritt Motor rechts von Pin D2
  #define MotEnable 4                                             // Enable beide Motoren (Treiber A4988 aktiviert wenn LOW) 
  
  /* Vorbereiten Kommunikation mit Gyrosensor MPU-6050: */
  #include "Wire.h"                                               // Bibliotheksprogramm fuer I2C-Verbindung (zum MPU-6050)
  #include "MPU6050_tockn.h"                                      // Bibliotheksprogramm fuer Funktion des MPU-6050
  MPU6050 mpu6050(Wire);
  
  /* Einige Parameter festlegen: */
  #define BALANCEWINKEL -108                                      // Neigungswinkel in Grad, welcher nah an der Balance liegt, abhaengig von der Anordnung 
                                                                                                
  /* Parameter für PID-Neigungsregelung:*/
  int kp= 80;                                                     // P-Anteil des Reglers (experimentell ermittelt)                                   
  int ki= 50;                                                     // I-Anteil des Reglers (experimentell ermittelt)      
  int kd= 200;                                                    // D-Anteil des Reglers (experimentell ermittelt) 
  #define GRENZWERT 4000                                          // Maximalwert fuer die Stellgroesse Neigungswinkel (pid), das ergibt ...
                                                                  // ...500000/4000 MicrosProSchritt, 8000 1/8 Schritte a 0,225 Grad/Sek = 5 Rad-Umdr/Sek
  /* Initialwerte vorgeben: */   
  int MotLinksGeschwind = 0;                                      // Initialwert 0, waere ein 1/8 Schritt a 0,225 Grad pro Sekunde, Wert wird aber dann...
  int MotRechtsGeschwind = 0;                                     // ...durch Stellgroesse pid veraendert   
  long Looptimer = 0;                                             // In dieser Variablen wird die Zeit gespeichert, die mit millis verglichen wird
  int Loopzeit = 10;                                              // Zeitabstand zwischen zwei Messungen am MPU6050 und Neuberechnung der Stellgroesse...                                                                                                                               
  long LetzteZeit = 0;                                            // ...dabei erfolgt aber keine Ausgabe von Schritten - optimale Loopzeit austesten                                               
  long Zeit = 0;                                                                                                   
  float Abweich = 0;                                              // Zeit und Abweichung wird benoetigt fuer den D-Anteil der Regelung 
  float LetzteAbweich = 0;                                                   
    
  float WinkelBalance;
  float WinkelY;                                                  // 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                                                                                           
  
  
  void setup()
  {
   Serial.begin(115200);
   
   pinMode(MotLinksD, OUTPUT);
   pinMode(MotLinksS, OUTPUT);
   pinMode(MotRechtsD, OUTPUT);
   pinMode(MotRechtsS, OUTPUT);  
   pinMode(MotEnable, OUTPUT);
  
   digitalWrite(MotEnable, LOW);                                  // Beide Motortreiber aktiv setzen
   delay(1000); 
   
   Wire.begin();                                                  // Initialisierung I2C Bus fuer Kommunikation mit MPU6050(Arduino als Master)
   Wire.setClock(400000);                                         // I2C Takt erhoehen von standardmaessig 100kHz auf 400kHz// Etwas warten, bis eventuelle Erschuetterungen abgeklungen sind              
   mpu6050.begin();                                               // Bibliotheksprogramm fuer den MPU startet, Segway nahe Balance-Winkel halten
   mpu6050.calcGyroOffsets(true);                                 // Waehrend der Kalibrierung nicht bewegen! 
   float WinkelY = mpu6050.getAngleY();                           // Bei anderer Ausrichtung des MPU-6050 muss andere Achse gewaehlt werden 
   WinkelBalance = WinkelY;                                       // 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         
   Serial.println();
   Serial.print("WinkelY bei Start= ");
   Serial.println(WinkelY);
  }
  
  
  void loop()
  {
   if(millis() > Looptimer)                                       // Der folgende Programmabschnitt erst wieder wenn Loopzeit vorbei ist...
   {                                                              // ...siehe Looptimer aktualisieren weiter unten  
    mpu6050.update();                                             // MPU-6050 auslesen
    float WinkelY = mpu6050.getAngleY();                          // Bei anderer Ausrichtung des MPU-6050 muss andere Achse gewaehlt werden
    Looptimer = millis() + Loopzeit;                              // Looptimer aktualisieren                                
    Zeit = millis(); 
             
    if (WinkelBalance < BALANCEWINKEL) WinkelBalance +=0.1;       // Allmaehliche Angleichung des Werts an BALANCEWINKEL nach dem Start...
    if (WinkelBalance > BALANCEWINKEL) WinkelBalance -=0.1;       // ... siehe oben
  
    /* Berechnung PID-Regler: */     
    Abweich = WinkelBalance - WinkelY;                            // Regelabweichung bestimmen  
    p = kp  *  Abweich;                                           // P-Anteil berechnen
    i = i+(ki * Abweich);                                         // I-Anteil berechnen 
    if(i > GRENZWERT)i = GRENZWERT;                               // Der I-Anteil wird begrenzt (ebenso wie der gesamte pid-Wert, siehe unten)
    if(i < -GRENZWERT)i = -GRENZWERT;                                                                                                           
    d = kd * ((Abweich-LetzteAbweich)/(Zeit-LetzteZeit));         // D-Anteil berechnen
    pid = p + d + i;                                              // Stellgroesse (fuer MotLinksGeschwind und MotRechtsGeschwind) berechnen
    LetzteAbweich = Abweich;
    LetzteZeit = Zeit;
    if(pid < 10 && pid > -10)pid = 0;                             // Bei geringer Abweichung von der Balance eine "Totzone" (vorerst nur gering)
      
  /* Weiterverarbeiten der Stellgroesse: */
    if(abs(pid) < GRENZWERT)                                      // Wenn die Stellgroesse innerhalb des erlaubten Bereiches liegt
    {
     MotLinksGeschwind = pid ;                                                         
     MotRechtsGeschwind = pid ;                             
    }                                                             // Wenn sie ausserhalb liegt, bleibt der bisherige Wert unveraendert
    if(WinkelY > -60 || WinkelY < -160) PORTD =PORTD | B00010000; // Falls der Roboter umgefallen ist, Motoren abschalten...*/
   }                                                              // ...entspricht digitalWrite(MotorEN,HIGH)                          
   
   /* Aufruf der Unterprogramme zur Ansteuerung der Motoren: */
   MotLinks();                                               
   MotRechts();  
  }
  
  
  void MotLinks()                                            
  {
   if(MotLinksGeschwind == 0) return;                             // Wenn Null - zurueck zu Beginn von void loop   
                                                                  // Wenn ungleich null:
   PORTB = PORTB & B11111101;                                     // D9 bzw A4988-Eingang STEP auf LOW setzen, durch Portmanipulation mit bitweise UND  
                                                                  // Wesentlich schneller als mit Befehl digitalWrite(Motor1S, LOW), aber notwendig noch...                                         
   delayMicroseconds(3);                                          // ...delay, weil der A4988-Eingang STEP eine min Impulslaenge 1,9 Mikrosek verlangt                                                                                                                                                           
   static unsigned long LetzterSchritt = 0;                       // Variable mit "static": Wert 0 wird nur beim ersten Durchlauf zugewiesen
   static boolean Status = false;                                 // false ist gleichbedeutend mit 0 bzw LOW                                                                                  
   long MicrosProSchritt = 500000ul / abs(MotLinksGeschwind);     // Berechnung der Zeit zwischen zwei Schritten (1/8 Schritten 0,225 Grad)                                                                                                                                   
   digitalWrite(MotLinksD, MotLinksGeschwind > 0 ? LOW:HIGH);     // Richtungsangabe setzen, Ausgabe an Pin Motor1D
                                                                  // Ergebnis ist HIGH, wenn Motor1Geschwind > 0 ist. Sonst LOW.
   if(micros() - LetzterSchritt > MicrosProSchritt)
   {                                                              // Wenn die Zeit ueberschritten ist erfolgt ein Wechsel des Status
    PORTB = PORTB | B00000010;                                    // D9 und damit A4988-Eingang STEP auf HIGH setzen, ein 1/8 Schritt 0,225 Grad wird ausgefuehrt 
                                                                  // Portmanipulation mit bitweise ODER
    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   
                                                                  // Wenn ungleich null:
   PORTD = PORTD & B11111011;                                     // D2 bzw A4988-Eingang STEP auf LOW setzen, durch Portmanipulation mit bitweise UND  
                                                                  // Wesentlich schneller als mit Befehl digitalWrite(Motor1S, LOW), aber notwendig noch...                                         
   delayMicroseconds(3);                                          // ...delay, weil der A4988-Eingang STEP eine min Impulslaenge 1,9 Mikrosek verlangt                                                                                                                                                           
   static unsigned long LetzterSchritt = 0;                       // Variable mit "static": Wert 0 wird nur beim ersten Durchlauf zugewiesen
   static boolean Status = false;                                 // false ist gleichbedeutend mit 0 bzw LOW                                                                                  
   long MicrosProSchritt = 500000ul / abs(MotRechtsGeschwind);    // Berechn der Zeit zwischen zwei Schritten (1/8 Schritten a 0,225 Grad)  
   //Serial.print ("MicrosProSchritt= ");
   //Serial.println (MicrosProSchritt);                                                                                                                                        
   digitalWrite(MotRechtsD, MotRechtsGeschwind > 0 ? HIGH:LOW);   // Anders als bei void MotLinks hier HIGH:LOW, da Drehrichtung invers sein muss...
                                                                  // ...weil die Achse in entgegensetzte Richt zeigt
   if(micros() - LetzterSchritt > MicrosProSchritt)
   {                                                              // Wenn die Zeit ueberschritten ist erfolgt ein Wechsel des Status
    PORTD = PORTD | B00000100;                                    // D2 und damit A4988-Eingang STEP auf HIGH setzen - ein 1/8 Schritt 0,225 Grad wird ausgefuehrt 
                                                                  // Portmanipulation mit bitweise ODER
    LetzterSchritt = micros();                                    // Variable wird wieder aktualisiert, fuer erneuten Vgl mit micros                                                         
   }
  }