sketch_88_Segway_Fahren

sketch_88_Segway_Fahren
  /* Komplette Funktion des Segway.
  Akku ausreichend geladen ? Min 8V notwendig fuer die A4988 und das bei Betrieb der Motoren. Siehe Voltmeter Anzeige am Segway. 
  Kommandos vom Smartphone, App "Bluetooth Terminal HC-05" (leider mit Einblendungen Werbung). 
  Koppeln HC-05 mit Smartphone: Siehe App "Bluetooth Terminal HC-05". 
  Baudrate zur UART-Bus Kommunikation mit HC-05 hoch einstellen: HC_Serial.begin(115200); muss im HC-05 ebenfalls so eingestellt worden sein.
  Siehe Sketch "HC05 konfigurieren".

  Der Segway wird etwa in Balance-Lage gehalten. Wenn das USB-Kabel angeschlossen ist, kommt nach Hochladen 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   (in Grad ; wenn Segway in dieser Lage gehalten wird)
  
  Wenn weitere Ausgaben am Serial Monitor gemacht werden sollen in der Loop-Schleife (aktuell nicht im Sketch enthalten): 
  Baudrate zum Serial Monitor sollte hoch sein fuer geringe Verlaengerung Programmlaufzeit.
  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 abhängig 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.
  MotEnable wird erst nach Kommando "e" auf LOW gelegt und damit die Motoren gestartet.
  Wenn MotEnable bereits im Setup auf LOW gelegt wird, muss bereits im Setup der WinkelY ermittelt werden "float WinkelY = mpu6050.getAngleY();"
  sowie auch "WinkelBalance = WinkelY;" stehen (wie im sketch_87_Segway_BalanceTest).      
  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 HC-05: */ 
  #include <SoftwareSerial.h>                                     // Bibliotheksprogramm fuer Anlage eines UART-Bus (zum HC-05)
  SoftwareSerial HC_Serial(6,7);                                  // Pin D6 wird RxD (an TxD des HC-05) und Pin D7 wird TxD (an RxD des HC-05) 
  
  /* 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 -107                                      // Neigungswinkel in Grad, welcher nah an der Balance liegt, abhaengig von der Anordnung 
  #define RICHTUNGAEND 200                                        // Wie schnell soll der Segway die Richtung aendern       
  #define NEIGUNG 3.0                                             // Mit diesem Wert wird der Neigungswinkel und damit die Geschwindigkeit verstellt... 
                                                                  // ...bei Fahrt auf Teppich die Neigung hoeher stellen (Kommando "v" mehrfach geben)                                                                                                         
  /* 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 für 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 und auch Vorgabe einer Drehung (RichtungAend) 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 Neigung = 0;
  float BeschleunNeigung = 0;                                     // Neigung fuer Anfahren (Beschleunigen)                                      
  float FahrNeigung = 0;                                          // Nach Beendigung des Anfahrens soll die Neigung wieder etwas verringert werden
  int RichtungAend = 0;                                           // Fuer Links- bzw Rechtsdrehung des Segway
  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);                                          // Serial Monitor entsprechend anpassen !
   HC_Serial.begin(115200);                                       // Beachte: Baudrate im HC-05 ebenfalls auf 115200 (im Sketch "HC05 konfigurieren")                                                                               
   HC_Serial.println("Start Programm"); 
    
   pinMode(MotLinksD, OUTPUT);
   pinMode(MotLinksS, OUTPUT);
   pinMode(MotRechtsD, OUTPUT);
   pinMode(MotRechtsS, OUTPUT);  
   pinMode(MotEnable, OUTPUT);
   
   digitalWrite(MotEnable, HIGH);                                 // Beide Motortreiber vorerst inaktiv setzen (aktivieren via Smartphone "e")
   delay(1000);                                                   // Etwas warten, bis eventuelle Erschuetterungen abgeklungen sind              
   Wire.begin();                                                  // Initialisierung I2C Bus fuer Kommunikation mit MPU6050(Arduino als Master)
   Wire.setClock(400000);                                         // I2C Takt erhoehen von standardmaessig 100kHz auf 400kHz
   mpu6050.begin();                                               // Bibliotheksprogramm fuer den MPU startet, Segway nahe Balance-Winkel halten
   mpu6050.calcGyroOffsets(true);                                 // Waehrend der Kalibrierung nicht bewegen! 
   HC_Serial.println("Bereit fuer Kommandos");                    // Ausgabe am Smartphone
   
   mpu6050.update();                                              // MPU-6050 auslesen
   float WinkelY = mpu6050.getAngleY();                           // Bei anderer Ausrichtung des MPU-6050 muss andere Achse gewaehlt werden
   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(); 

    /* Abfrage und Auswertung von Eingaben vom Smartphone: */   
    while(HC_Serial.available())
    {
     char Eingabe = HC_Serial.read();                             // Abfrage, ob ein Kommando vom Smartphone kam
     if(Eingabe < ' ')continue;                                   // Solange keine Eingabe kommt, werden alle folgenden if-Abfragen uebersprungen
     if(Eingabe == 'v') BeschleunNeigung += NEIGUNG;              // Jede Eingabe "v" erhoeht den Neigungswinkel und damit die Geschwind nach vorn                
     FahrNeigung = BeschleunNeigung * 0.8;                        // Nach Abschluss der Beschleunigung wird die Neigung etwas reduziert (siehe unten) 
     if(Eingabe == 'h') BeschleunNeigung -= NEIGUNG;              // Wie oben, aber nach hinten
     FahrNeigung = BeschleunNeigung * 0.8; 
     if(Eingabe == 's') BeschleunNeigung = 0;                     // Neigung zu 0 Grad setzen, d.h. Zielwert wieder fuer BALANCEWINKEL  
     if(Eingabe == 'l') RichtungAend = RICHTUNGAEND;              // Rechtsdrehung (staendig) mit dem Wert in RICHTUNGAEND       
     if(Eingabe == 'r') RichtungAend = -RICHTUNGAEND;;            // Linksdrehung (staendig) mit dem Wert in RICHTUNGAEND         
     if(Eingabe == 'o') RichtungAend = 0;                         // Keine weitere Drehung  
     if(Eingabe == 'e')                                           // Mit Kommando "e" werden die Motoren eingeschaltet 
     {
      WinkelBalance = WinkelY;                                    // Wenn beim Start der Segway nicht genau auf BALANCEWINKEL steht, wird die Regelabweichung... 
      Neigung = 0.0;                                              // ...sofort hoch, die Raeder drehen schnell. Der Wert wird vorerst dem tatsaechl Winkel angepasst                                                                              
      BeschleunNeigung = 0.0;
      RichtungAend = 0.0;
      i = 0;                                                      // I-Anteil (gespeichert in Variable i) nullen
      digitalWrite(MotEnable, LOW);                               // Motoren einschalten (an Eingang /EN am Treiber-Modul mit A4988)                   
     }
      if(Eingabe == 'a') digitalWrite(MotEnable, HIGH);           // Mit Kommando "a" werden die Motoren abgeschaltet      
    }
    
    /* Fuehrungsfunktionen fuer Regler-Sollwertvorgaben: */     
    if(BeschleunNeigung > 0.00)
    { 
     if(Neigung < BeschleunNeigung) Neigung +=0.02;               // Der Sollwert der Neigung wird bei jedem Programmdurchlauf etwas erhoeht
     else BeschleunNeigung = FahrNeigung;                         // Bei Loopzeit 10ms wird Sollwert Neigung 3 Grad nach 1,5 Sek erreicht 
     if(Neigung > FahrNeigung) Neigung -=0.02;                    // Nach der Beschleunigungsphase wird er wieder schrittweise etwas reduziert 
    }
    if(BeschleunNeigung < 0.00)
    {
     if(Neigung > BeschleunNeigung) Neigung -=0.02;
     else BeschleunNeigung = FahrNeigung;
     if(Neigung < FahrNeigung) Neigung +=0.02;   
    }  
    if (BeschleunNeigung==0)
    {
     if(Neigung < 0) Neigung +=0.02;                              // Der Sollwert der Neigung wird bei jedem Programmdurchlauf etwas erhoeht...   
     if(Neigung > 0) Neigung -=0.02;                              // ...bzw verringert, dadurch wird einem Wegdriften entgegengewirkt sowie...
    }                                                             // ...nach Kommando "s" allmaehlich gebremst
    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 + Neigung - 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 + RichtungAend ;                                                         
     MotRechtsGeschwind = pid - RichtungAend ;                             
    }                                                             // 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                                                         
   }
  }