/*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
}
}