/* 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= -105 (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 -105 // 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 2.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
}
}