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