Projekt 57: Arduino Segway
Autoren: Philipp Tewes, Janis Ostermann, Gaida Sven
Betreuer: Prof. Dr. Mirek Göbel
Das Projekt
Das Projekt wurde von Philipp Tewes und Janis Ostermann im Rahmen des Elektrotechnik Fachpraktikums im WS 2015/16 bearbeitet. Das Ziel sollte es sein, ein Segway auf der Basis eines Arduino UNO's zu entwerfen, welches sich selbst balancieren sollte um somit ohne Hilfe stehen zu können. Bevor mit dem Bau und der Programmierung des Segways begonnen wurde, sollten erst einmal einige Ziele definiert werden. Zuerst sollte die Hardware des Segways geplant werden. Dazu wurde eine Teile-Liste angefertigt. Nachdem alle benötigten Teile eingetroffen waren, wurde mit dem Bau der einzelnen Etagen begonnen. Fortgesetzt wurde die Arbeit mit dem Bau der Steuerbox welche den Gyro Sensor und die Potentiometer enthalten sollte. Die Potentiometer sollten zur einfachen Regulierung der Regelparameter dienen.
Nachdem alle Einzelteile fertiggestellt waren ging es um die Verkabelung der Komponenten. Nach Montage und der Verkabelung wurde das Segway anschließend durch Anbringung der Räder fertig gestellt.
Anschließend bestand die Aufgabe darin eine passende Software für die Regelung des Segways zu entwerfen und abzustimmen.
Weitergeführt wurde das Projekt im Wintersemester 2017/2018 von Sven Gaida und Philipp Tewes. Hierbei wurden die Punkte verfolgt die im letzten Bearbeitungs-Zeitraum nicht umgesetzt werden konnten. Umgesetzt wurden die Fernsteuerung durch eine Fernbedienung die mit Wifi mit dem Segway verbunden wird und die überarbeitung des Regelers und die Radbefestigungen wurden überarbeitet.
Bau
Teile-Liste
- 2x RC Reifen 1:8 (Durchmesser 10cm)
- 1x Bastelglas 25cm x 34cm
- 1x Holzstab 70cm x 1,2cm
- 1x Arduino Uno R3
- 1x Saint Smart Sensor Shield
- 2x Motor Drive L298N
- 1x Beschleunigungs- und Gyro Sensor MPU-6050
- 2x Getriebemotor 12V DC MFA 919D1481 148:1
- 3x Potentiometer 10K
- 1x Ladegerät APC300
- 1x Akku-Pack PP2
- 2x Achskupplung (M5 auf M5)
- 2x Gummi Unterlegscheibe (Innendurchmesser 0,4cm Außendurchmesser 1,7cm)
- 7x Metall Unterlegscheibe (Innendurchmesser 0,8cm Außendurchmesser 2,3cm)
- 3x Metallschrauben M5 Länge 3cm
- 4x M5 Unterlegscheiben
- 10x Isolierende Gummi-Abstandsscheiben (für M3 Schrauben, 3mm Stärke)
- 1x M5 Mutter
- 1x M4 Gewindestange 1m
- 24x M4 Muttern
- 1x Tamya Stecker mit 5cm Kabel
- 18x M3 Schrauben (1,5cm) mit Muttern
- 4x Kleine Nägel
- 4x Kleine Schrauben
Etagen
Beim Aufbau des Segways wurde sich für ein Konzept aus drei Etagen entschieden. Dabei diente Etage eins für die Befestigung des Holzstiels mit der Steuerbox und dem Lenker. Etage zwei war dabei die Etage an der die MotorShields, der Arduino UNO R3 und die Motoren verschraubt wurden. Etage drei diente dabei als Auflage für das Akku Pack. Beim Aufbau wurde darauf geachtet, dass der Schwerpunkt möglichst tief und unter der Fahrzeugachse lag. Daher befanden sich das Akku Pack und der Motor unter der Fahrzeugachse. Dadurch entstand ein Pendeleffekt der das Segway stabilisierte.
Etage 1
- 3mm Bohrungen für M4 Gewindestangen
- Langloch für die Bohrung eingeschnitten
Etage 2
- 3mm Bohrungen für M4 Gewindestangen
- 2mm Bohrungen für Motoren und Motor-Shield
- Rote Löcher für Motor-Shields, nur Außen bemaßt. Innere Löcher können je nach Shield abweichen. Außen wurde gebohrt und dann Innen entsprechend angezeichnet
- Grüne Löcher für Motoren, nur Außen bemaßt. Innere Löcher können je nach Motor abweichen. Außen wurde gebohrt und dann innen entsprechend angezeichnet
- Die Löcher für die Befestigungen der elektrischen Bauteile wurden gesenkt
- Arduino nur zur Kannte bemaßt, mittig aufgesetzt und angezeichnet
Etage 3
- 3mm Bohrungen für M4 Gewindestandgen
- Gummi überziehen
Steuerbox
- 5mm Löcher in das Plexiglas für die Potentiometer gebohrt
- Potentiometer in das Plexiglas eingeschraubt. Über Gewinde am Potentiometer.
- Folgende Komponenten mit einer Gummi Abstandsscheibe versehen
- Gyroskop an die mit 3mm vorgeborten Löcher mit M3 Schrauben geschraubt und mit Mutter auf der Unterseite befestigt
- Platine Ebenfalls mit M3 Schrauben verschraubt und befestigt
- In die Unterseite ein 9mm Loch gebohrt um die Kabel herausführen zu können
- Zusätzlich wurde eine kleine Schaltung entworfen. Diese besaß einen 10 nanofarad Keramik Kondensator (C1) zwischen Plus und Minus für die Entstörung. Ein weiterer 10 Mikrofarad Elektrolyt Kondensator (C2) war dabei für eine Strom Speicherung für den Gyrosensor vorhanden. Denn der Gyrosensor benötigte eine sehr stabile Spannung um fehlerfreie Werte liefern zu können.
Stiel und Querstange
- Holzstiel in zwei Stücke geschnitten, einmal 50,5cm und 14cm
- Den 14cm Stiel mittig in eine Stirnfläche des längeren Stiels verschrauben, mit M5 Schraube 3cm und M5 Unterlegscheibe (vorgebohrt)
Verkabelung
Die Verkabelung wurde in Fritzing geplant und wie in der Abbildung umgesetzt, einzig das Sensor Shield stand dort nicht zur Verfügung. Dies stellte aber kein Problem bei der Planung der Verkabelung dar, denn auf dem Sensor Shield gab es einen extra Anschluss für den Gyro Sensor. Bei den Motoren wurde dabei auf ein Kabel mit 0,5mm Querschnitt gesetzt, für die restlichen Kabel 0,25mm. Der Akku wurde dabei mit dem Tamya Stecker und 5cm Kabel in die Klemmen des rechten Shields geklemmt. Von hier aus gingen noch zwei Kabel zum linken Shield, um dieses ebenfalls mit Spannung vom Akku zu versorgen. Die Kabel der Steuerbox wurden von der Bohrung oder der Bodenplatte der Steuerbox aus am Stiel verlegt, an diesem wurden sie mit Kabelbindern befestigt. Bevor die Verkablung nach dem Fritzing Plan vorgenommen wurde, musste das Sensor Shield auf den Arduino UNO aufgesteckt werden.
Reifen
Weil es sich bei den Reifen um RC Reifen in der Größe 1:8 handelte waren diese nicht für den Betrieb an den Getriebemotoren vorgesehen. Aus diesem Grund musste ein eigener kleiner Adapter nach der folgenden Abbildung zusammengebaut werden. Bei dieser Verbindung war auf besonders hohe Stabilität zu achten, denn sie bildeten gleichzeitig die Achse. Die Schrauben wurden dabei noch an einer Stelle etwa 1mm mit einer Pfeile abgeflacht. So wurde sichegestell, dass sich die Achse nicht durch das Anfahrdrehmoment der Motoren verdrehen konnte. An den Motoren war diese Abflachung bereits vorhanden. Die Achskupplungen wurden auf die Schrauben gesteckt. Diese sollten dabei auf die Abflachungen Radschrauben geschraubt werden.
Montage
Begonnen wurde mit der Montage des Stiels, nach dem Vorbild der ersichtlichen Zeichnung. In diesem Schritt wurde gleich die Steuerbox befestigt. Dies wurde mit einer M5 Schraube in 3cm umgesetzt. Zusätzlich wurde die Verschraubung auf der Außenseite am Stiel mit einer Unterlegscheibe versehen. Daraufhin wurden zunächst die Motoren an der Etage 2 befestigt. Dabei war zu beachten, dass zunächst alle Schrauben in die zugehörigen Löcher gesteckt werden mussten, da die Halterung der Motoren die Löcher daraufhin bedeckte. Danach wurde die restliche Hardware mit den M3 Schrauben befestigt (Arduino UNO, Motor Shields). Auf diese wurde pro Schraube eine Gummi Abstandsscheibe gesteckt. Auf Etage drei wurde noch ein Gummi gespannt, dies sollte ein Verrutschen des Akkus verhindern. Dann wurden Etage zwei und Etage drei mit einem Abstand von 7cm mithilfe der M4 Gewindestangen verbunden. Die Etagen wurden dabei durch die M4 Muttern positioniert und von der Gegenseite mit einer weiteren M4 Mutter leicht festgeklemmt. Der bereits vormontierte Stiel mit der Steuerbox wurde mit einer 3cm M5 Schraube an Etage eins befestigt. Dabei wurden auf beiden Seiten Unterlegscheiben untergelegt, um die Belastung auf das Glas zu verteilen. Bevor Etage eins ebenfalls verschraubt wurde mussten noch die Kabel des Stiels auf dem Sensor Shield versteckt werden. Jetzt konnte auch Etage eins mit den restlichen Etagen über die Gewindestangen verschraubt werden. Auch hier mit einem Abstand von je 7cm. Als letzter Schritt mussten noch die Räder an den Motoren befestigt werden. Hierzu wurden die Achskupplungen auf die Motoren gesteckt und auch dort durch die Schrauben der Achskupplungen festgeschraubt.
Software des Arduino UNO
Die folgende Software wurde mit der Arduino IDE Entwicklungsumgebung entwickelt. Es wurden einige Bibliotheken eingebunden, die bei der Kommunikation mit dem Gyrosensor helfen sollten. Zunächst sollten die gegebenen Offsets für unseren Gyrosensor herausgefunden werden. Dazu wurde ein Beispielprogramm genutzt, welches die aktuellen Gyroskop-Werte ausgab. Der aktuelle Wert im gerade stehenden Zustand wurde als Referenzwert, in welchen sich das Segway zu begeben versuchen sollte, gewählt. Der Hauptteil des Programms maß ständig die Winkeländerung und ermittelte über die vergangene Zeit den aktuellen Winkel. Sobald dieser über 45 Grad groß wurde, wurde nichts weiter ausgeführt um so beim Umfallen des Segway die Motoren zu stoppen. Solange aber der Winkel unter 45 Grad groß war sollte das Segway sich wieder in eine gerade Position bringen. Dieses sollte durch ansteuern der Motoren passieren, welche durch die Fahrt in die Richtung in welche das Segway kippte, dieses wieder aufstellten. Die Kraft mit der die Motoren angesteuert werden sollten wurde mit einem PID Reglerberechnet. Dieser bekam als Eingangsgröße den aktuellen Abweichungswinkel und hatte die Kraft mit dem die Motoren angesteuert wurden als Ausgangsgröße. Die Werte mit denen der Regler laufen sollte wurden über feste Variablen definiert oder über die analogen Eingänge des Arduinos von den Potentiometern eingelesen.
Im Folgenden ist unser Programm angehängen. Die Bibliotheken können aus dem Internet oder dem Programmordner im zugehörigen SVN Verzeichnis bezogen werden.
Programmcode
#include "Wire.h"
#include "SPI.h"
#include "nRF24L01.h"
#include "I2Cdev.h"
#include "MPU6050.h"
// Variablen für das Accelometer und den Gyro
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
// Definitionen von Schlüsselwörtern
#define Gry_offset 300 // Offset des Gyro
#define Gyro_Verstaerkung 0.00763358 // Verstärkung des Gyro Analog Signals
#define Winkel_offset -22 // Offset des aktuellen Winkels
#define Motor_offset 0 // Motor Offset
#define pi 3.14159 // Definition von der Zahl Pi
// Variablen für den PID Regler
float kp, ki, kd;
float r_angle, f_angle, omega;
// Variablen für die Zeitberechnung
unsigned long preTime = 0;
float SampleTime = 0.08;
unsigned long lastTime;
float Output;
float errSum, dErr, error, lastErr;
int timeChange;
// Definition der Ports mit Variable für Pinmode
//Reifen rechts
int TN1=8;
int TN2=9;
int ENA=5;
//Reifen links
int TN3=11;
int TN4=10;
int ENB=6;
int sensorPin0 = A0; // Input Pin für das Potentiometer 0
int sensorPin1 = A1; // Input Pin für das Potentiometer 1
int sensorPin2 = A2; // Input Pin für das Potentiometer 2
int sensorValue1 = 0; // Variable um den Wert des Potentiometer 0 zu speichern
int sensorValue0 = 0; // Variable um den Wert des Potentiometer 1 zu speichern
int sensorValue2 = 0; // Variable um den Wert des Potentiometer 2 zu speichern
void setup() {
Wire.begin(); // Start serieller Kommunikation
accelgyro.initialize(); // Initalisieren des Gyro/Accelometer
pinMode(TN1,OUTPUT); // Ausgänge zuweisen
pinMode(TN2,OUTPUT);
pinMode(TN3,OUTPUT);
pinMode(TN4,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
Serial.begin(115200); // Serielle Bandrate
}
void loop()
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // Aktueller Wert des Gyro/Accelometer laden
r_angle = (atan2(ay, az) * 180 / pi + Winkel_offset); // Aktueller Winkel (bei 0 Grad steht das Segway gerade)
omega = Gyro_Verstaerkung * (gx + Gry_offset); Serial.print(" omega="); Serial.println(omega);
if (abs(r_angle)<45){ // Winkel kleiner als 45 Grad
myPID(); // Berechnung der aktuellen PID Werte
PWMControl(); // Ansteuern der Motoren mit aktuellen Werten
}
else{ // Wenn der winkel größer als 45 Grad wird soll angenommen werden, dass das Segway umgefallen ist und keine Bewegung mehr ausgeführt werden
analogWrite(ENA, 0); // Motor A Bremsen
analogWrite(ENB, 0); // Motor B Bremsen
}
}
// PID Werte Berechnung
void myPID()
{
kp = analogRead(A0)*0.1; Serial.print("kp= ");Serial.print(kp); //Einlesen des kp Wertes über Potentiometer wählbar
ki = analogRead(A1)*0.001; Serial.print(" /// ki= ");Serial.print(ki); //Einlesen des ki Wertes über Potentiometer wählbar
kd = analogRead(A2)*1.5; Serial.print(" /// kd= ");Serial.println(kd); //Einlesen des kd Wertes über Potentiometer wählbar
// Wahlweise können auch feste Werte genutzt werden
//kp = 35; Serial.print(" kp=");Serial.print(kp);
//kd = 30; Serial.print(" kd=");Serial.print(kd);
//ki = 0; Serial.print(" ki=");Serial.println(ki);
unsigned long now = millis(); //aktuelle Zeit
float dt = (now - preTime) / 1000.0; //vergangene Zeit seit vor der letzten Berechnung
preTime = now; //Zeit zu Beginn
float K = 0.8;
float A = K / (K + dt);
f_angle = A * (f_angle + omega * dt) + (1 - A) * r_angle; Serial.print(" f_angle=");Serial.println(f_angle); //Winkel Berechnung
timeChange = (now - lastTime); //vergangene Zeit seit Beginn der letzten Berechnung
if(timeChange >= SampleTime){
error = f_angle; // Abweichung von 0 Grad
errSum += error * timeChange;
dErr = (error - lastErr) / timeChange;
Output = kp * error + ki * errSum + kd * dErr; Serial.print(" Output=");Serial.println(Output);
lastErr = error;
lastTime = now; //Zeitstempel nach Berechnung
}
}
void PWMControl(){
if(Output > 0){ //Vorwärts
digitalWrite(TN1, HIGH);
digitalWrite(TN2, LOW);
digitalWrite(TN3, HIGH);
digitalWrite(TN4, LOW);
}
else if(Output < 0){ //Rückwärts
digitalWrite(TN1, LOW);
digitalWrite(TN2, HIGH);
digitalWrite(TN3, LOW);
digitalWrite(TN4, HIGH);
}
else{ //Bremse
digitalWrite(TN1, HIGH);
digitalWrite(TN2, HIGH);
digitalWrite(TN3, HIGH);
digitalWrite(TN4, HIGH);
}
analogWrite(ENA, min(255, abs(Output) + Motor_offset)); //Geschwindigkeit rechts
analogWrite(ENB, min(255, abs(Output) + Motor_offset)); //Geschwindigkeit links
}
Regler Auslegung
Die Abstimmung des realisierten PID Reglers wurde, wie aus der Vorlesung der Reglungstechnik bekannt, mit der Schwingungsmethode vorgenommen. Vorgegangen wurde dabei nach den folgenden fünf Schritten.
PID Reglerauslegung mit Schwingungsmethode
Ziegler/Nichols
Vorgehen:
- Parametrierung des Systems als reiner P‐Regler. Dabei wurden die Anteile Ki und Kd auf null gesetzt
- Erhöhung der Regelverstärkung (Kp-Wert), bis Dauerschwingungen auftreten
- Bestimmen der Regelverstärkung Kp krit, und der Periodendauer T krit
- PID‐Reglerparameter aus der Tabelle des Einstellverfahrens ablesen. (siehe Tabelle)
- Test des eingestellten Reglers
| Reglertypen | |||
|---|---|---|---|
| Kp | Ki | Kd | |
| P | 0,5 Kp krit | - | - |
| PI | 0,45 Kp krit | 0,85 T krit | - |
| PID | 0,6 Kp krit | 0,5 T krit | 0,12 T krit |
Achtung: Es musste darauf geachtet werden, das das Segway beim Überschwingen nicht umfällt und beschädigt wird!
Fazit
Abschießend kann man sagen, dass das Projekt viel Zeit gekostet hat, dieses aber durch Spaß an der praktischen Arbeit und am fertigen, funktionstüchtigen Projekt gut gemacht wurde.
Probleme
Im Laufe des Projekts kam es immer wieder zu kleineren Problemen:
- Das erste Motorshield überhitzte uns einige Male bis zum Defekt, da die Anlaufströme der Motoren zu groß waren. Da beim Regeln immer wieder Angefahren wurde kam es häufig zu zu hohen Anlaufströmen. Dies wurde durch die Verwendung eines zweiten Motorshields gelöst. Die Hitze konnte so besser abgeführt werden, da die Leistung auf zwei Bausteine getrennt wurde.
- Sich lösende Schrauben an den Achsverbindern führten immer wieder dazu, dass das Segway die Kraft der Motoren nicht gleichmäßig auf den Untergrund übertragen konnte und so aus dem Gleichgewicht kam. Dieses Problem wurde bisher noch nicht gelöst.
- Da der aktuelle Akku-Ladestand nicht angezeigt wurde, war es nie klar wann der Akku bei der Ladung genau voll war um so einer Überladung vorzubeugen. Genauso war es nicht klar wann der Akku zu leer sein würde um das Segway zu betreiben oder sich beschädigen würde durch zu hohe Entladung.
Erreichte Ziele
Es wurden alle von uns gestellten Ziele erreicht. Die Hardware wurde von uns selbst entwickelt und ist voll funktionstüchtig. Außerdem konnte eine laufende Software geschrieben werden, die das Segway sich selbst balancieren ließ.
Fortführung
- Es besteht die Möglichkeit der Erweiterung durch eine Fernbedienung, zum Beispiel per Bluetooth oder WLAN, durch welche mit einem passenden Modul mit dem Arduino kommuniziert werden könnte
- Es könnte, da die Motoren getrennt von einander angesteuert werden können, eine Lenkbewegung programmiert werden, mit der sich das Segway fortbewegen und geteuert werden könnte
- Leider ist es noch nicht gelungen die idealen Werte für den PID Regler zu finden. Die Reglung könnte noch so weit optimiert werden, dass das Segway auch wenn es stärker angestoßen würde, sich wieder fangen könnte
Segway 2.0 mit FernDuino WS1718
Wie aus dem Ausblick des Artikels zum Segway aus dem Wintersemester 2015 ersichtlich wird, sollte das Projekt zu einem späteren Zeitpunkt weitergeführt werden. Das Segway 1.0 bietet noch weiteren Spielraum für Verbesserungen, die im Betrieb der Neukonstruktion ersichtlich wurden.
Folgende Punkte wurden bearbeitet:
- Neukonstruktion eines Segways 2.0 unter der Berücksichtigung der beim Segway 1.0 aufgetretenen Probleme.
- Überarbeitung der Motoraufhängung
- Überarbeitung der Radbefestigung
- Überarbeitung / Tausch der Motortreiber
- Bessere Ausbalancierung bei gleichzeitig "Segway näherem Design"
- Mehr Laufzeit durch die Möglichkeit paralleler Akkus
- Überarbeitung der Software, denn das Segway 1.0 leidet noch an zahlreichen Software Problemen
- DKS System um Hindernisse zu erkennen und einen Zusammenstoß aktiv verhindern/abschwächen zu können
- Konstruktion einer Fernbedienung um das Segway steuern zu können (FernDuino)
- Funkverbindung zwischen dem FernDuino und dem Segway
- Bewegungs- und Reglerparametersteuerung mit den analogen Joysticks
- Bewegungssteuerung per Neigungswinkeländerung
- Visualisierung der Reglerparameter und Bewegungssteuerungsart
- EEPROM Speicher für die Reglerparameter
- Einfaches Aufladen des Akkus per Tamiyastecker
Segway 2.0
Konstruktion
Wie bereits erwähnt handelt es sich beim Segway 2.0 um eine Neukonstruktion, die auf der Konstruktion des Segway 1.0 aufbaut. Hierbei stand es im Focus das Segway zuverlässiger zu gestalten und aus den aufgetretenen Fehlern zu lernen. Die Komponenten, die eine hohe Zuverlässigkeit im Fahrbetrieb boten, sollten übernommen werden.
Hierbei handelt es sich um die Komponenten:
- Der verwendeten Getriebemotoren
- Des Chassis aus Gewindestangen und Plexiglas
- Der verwendeten RC Reifen
Bei der Konstruktion wurden auch die weiter verwendeten Bauteile in ihrer Positionierung und Integration optimiert.
Radaufhängung
Die Radaufhängung bot beim Segway 1.0 gleich zwei Problemstellen.
Einmal war dies bei der Adaption der Motorachse und der Radachse, diese Stelle die geklemmt ausgeführt ist, löste sich gelegentlich an der Radachse. Dies konnte dadurch behoben werden, dass durch die Radachse hindurch eine Bohrung gemacht wurde. Gleichzeitig wurde die sehr kurze Schraube der Klemme durch eine längere ausgetauscht, die jetzt durch die Radachse geschraubt wird. Hierdurch ist ein fester Sitz der Achse sichergestellt.
Neben der Achsen wurde auch die Befestigung der Motoren am Chassis verbessert. Im Fall des Segway 1.0 wurden die Motoren an eine der Plexiglas Etagen verschraubt, damit lastete das gesamte Gewicht des Segways an dieser Etage. Hierdurch haben sich im Segway 1.0 bereits Risse in dieser Etage gebildet. Beim Segway 2.0 wurde aus diesem Grund ein neues Konzept entwickelt, bei diesem sollte nicht mehr eine der Etagen die Last aufnehmen müssen. Das Tragende Element sind im Segway 2.0 die Gewindestangen, die alle Etagen miteinander verbinden. Sie können die nötige Stabilität für die Motoren und der Etagen sicherstellen und wurden aus diesem Grund gleich achtfach verwendet. Verschraubt werden sie direkt mit der Aufhängung der Motoren und jeder Etage. So wird sichergestellt, dass sich die Belastungen auf alle Etagen gleichmäßig verteilen und so nicht zur Überlastung einer einzelnen Etage kommen kann.
Positionierung des MPU6050
Neben diesen Elementen wurde ebenfalls wieder der Gyroskop und Beschleunigungssensor MPU6050 eingesetzt. Es stellte sich bei Tests aber heraus, dass die Integration in die Box mit den Potentiometern nicht optimal gewählt war. Die Werte des Gyroskop wurden am exaktesten, wenn diese direkt in die Drehachse des Segways verbaut wurden. Die Werte weit oberhalb der Drehachse in der Steuerbox waren deshalb nicht aussagekräftig über das Verhalten in der Drehachse des Segways. Aus diesem Grund wurde der MPU6050 direkt in die Drehachse des Segway 2.0 verbaut, hierdurch konnten deutlich bessere Ergebnisse in der Regelung erzielt werden. Ebenfalls konnte die Filterung des Wertes minimiert werden, denn war der MPU6050 in der Box oberhalb des Stil einem ständigen schwanken ausgesetzt. Dieses führt zu Wertänderungen, die in dieser Ausgeprägtheit nicht in der Drehachse des Segway vorlagen.
Positionierung der Akkus
Bei dem Segway 2.0 wurde auf identische Akkus wie im Fall des Segway 1.0 gesetzt, mit dem Unterschied, dass beim neuen Segway zwei Akkus parallel geschlossen werden können. Hier für wurde die Positionierung der Akkus verändert. Die Akkus befindet sich nicht mehr zwischen den Motoren, sondern sitzen oberhalb beider Motoren. Auf diese weise bietet das Segway 2.0 Platz für bis zu zwei Akkus. Ziel hiervon war es die maximale Laufzeit des neuen Segways deutlich verlängern zu können, um so mit der Fernbedienung einen längeren Fahrspaß zu ermöglichen.
Steuerung des Segways
Wie bei der Konstruktion sollten auch bei den Steuerungs-Komponenten auf bewährte Bauteile zurückgegriffen werden. Dies umfasst:
- Den Gyroskop und Beschleunigungssensor MPU6050
- Die Steuerung über einen Arduino
Ergänzend wurden verbaut:
- Zwei SR04 Ultraschallsensoren
- Ein nRF24L01 Wifi Modul
Arduino Mega 2560
Bei der Rechenzentrale des Segway 2.0 handelt es sich im Gegensatz zum Segway 1.0 mit Arduino Uno, um einen Arduino Mega 2560. Diese Entscheidung wurde getroffen, um den zusätzlichen Sensoren und einer möglichen Erweiterung in der Zukunft genug Ports zur Verfügung zu stellen. Neben diesem Vorteil bietet der Arduino Mega mehr Speicher für Code und damit ausführlichere Programme. Die wichtigsten Daten des Arduino Mega sind folgend aufgeführt:
- Mikrocontroller: ATmega2560
- Flash-Speicher: 256KB
- SRAM: 8KB
- EEPROM: 4KB
- Taktrate: 16MHz
- I/O-Pins: 54, davon 15 PWM und 16 analoge Eingänge
- Anschluss: USB
- LEDs: RXD, TXD, Power, Pin 13
- Größe: ca. 10,2cm x 5,3cm
- Betriebsspannung: 5V
- Empfohlene Eingangsspannung: 7-12V
- Maximale Eingangsspannung: 20V
- Maximaler Strom pro I/O-Pin: 40mA
- Belastbarkeit des 3.3V Ausgangs: 50mA
SR04 Ultraschallsensor
Um die DKS Funktion beim Segway 2.0 ermöglichen zu können, musste ein Sensor verbaut werden mit welchem es möglich ist Objekte zu erkennen. Bei der Abkürzung DKS handelt es sich um ein Distanz Kontrolle System, dieses soll verhindern, dass das Segway mit anderen Objekten kollidiert. Um einen sicheren Stand des Segways sicherstellen zu können, bedarf es durchgängiger kleiner Fahrmanöver. Aus diesem Grund wurde das Segway so Programmiert, dass es zu anderen Objekten einen minimalen Sicherheitsabstand von 5cm einhält und es erkennt wenn es auf ein Hindernis auffährt. Hierdurch ist es möglich eine Kollision mit anderen Objekten durch Unachtsamkeit des Fahrer zu verhindern oder abzuschwächen. Bei dem hierfür verwendeten Sensor handelt es sich um einen HC_SR04 Ultraschallsensor, welcher die folgenden technischen Spezifikationen aufweist:
- Betriebsspannung: 5V
- Strombedarf: ca. 2mA pro Messung
- Signal Level: TTL-Pegel
- max. messbare Entfernung: ca. 3m
- min. messbare Entfernung: ca. 2cm
- Genauigkeit: ca. 3mm
- Maximale Messungen pro Sekunde: 50
nRF24L01 Wifi Modul
Für die Kommunikation zwischen dem Segway 2.0 und des FernDuinos 1.0 wurde sich für eine Wifi Verbindung entschieden. Diese verbindet eine gute Sendereichweite mit einer hohen Datenrate, gleichzeitig ist die Störanfälligkeit sehr gering. Verbaut wurde es oberhalb der restlichen Elektronischen Bauteile des Segways um eine mögliche Störung so gering wie möglich zu halten und den Empfang nicht zu verschlechtern. Bei dem verwendeten Modul handelt es sich um ein nRF24L01 Wifi Modul mit den folgenden technischen Spezifikationen:
- Betriebsspannung: 3.3V
- max. Reichweite: ca. 100m
- Anzahl Kanäle: 128
Genauere Informationen folgen im Abschnitt des FernDuino.
Schaltplan
Der Schaltplan für das Segway 2.0 wurde mit der freien Software Frizing entworfen. Hierdurch war schon im Vorfeld eine genaue Planung möglich und nötige PWM oder I/O Ports konnten auf ihre Verfügbarkeit überprüft werden. Anhand des Schaltplans ist es gut ersichtlich, dass beim Arduino Mega noch deutliche Reserven für die Zukunft eingeplant wurden, um auf einen späteren Tausch des Arduino Modells und damit auf eine weiteren Neukonstruktion verzichten zu können. Die Spannungsversorgungen werden dabei von den Bauteilen selbst zu Verfügung gestellt, so konnte auf zusätzliche Spannungswandler verzichtet werden. Die Ausgangsspamnnung von 9.6V der Akkus wird an die Motortreiber angelegt, mit welcher die Getriebemotoren betrieben werden. Auf den Motortreibern befinden sich Spannungswandler mit einer Ausgangsspannung von 5V mit welcher der Arduino Mega, die SR04 und der MPU6050 versorgt werden. Die nötige Spannungsversorgung von 3,3V für das Wifi Modul werden vom Arduino Mega erzeugt. Hiermit sind alle Bauteile mit der richtigen Betriebsspannung versorgt. Die 9.6V Akkus wurden im Schaltplan durch die beiden battery cases dargestellt.
Software
Bei der Software waren wie im Fall der Konstruktion deutliche Verbesserungen nötig. So war es zuvor nicht möglich die Getriebemotoren einzeln ansteuern zu können, denn ein Lenken des Segways war zuvor keine Anforderung. Durch die Integration des FernDuinos hatte sich dies geändert und eine einzelne Ansteuerung musste implementiert werden. Ebenfalls musste das Gegenstück des FernDuinos und damit der Empfang der Daten für den Fahrbetrieb und der Reglerwerte implementiert werden. Im Segway 1.0 wurden alle Werte für den Regler mit den Potentiometern der Steuerbox eingestellt und vom Arduino auf dem Segway selbst eingelesen. Aus dem Grund, dass im Fall der Steuerbox das Segway 1.0 für jede Wert Anpassung angefasst werden musste, wodurch sein Schwingverhalten beeinflusst wurde, sollte die Einstellung über den FernDuino stattfinden. Hierzu werden die Reglerwerte von dem FernDuino empfangen. Die Werte von -255 bis + 255 werden dabei vom Segway in einen geeigneten Winkel Offset umgerechnet, hierdurch kippt das Segway in die jeweilige Richtung und setzt sich in Fahrt. Auf eine Lösung eines Motor Offset, wie sie in anderen Arduino Segway Projekten zu finden ist, wurde bewusst verzichtet. Hierbei arbeitet der Regler gegen eine bewusst eingerechnete Störgröße und eine übergreifende Reglerparametrierung kann nicht eingesetzt werden. Für die nötigen Lenk-Bewegungen wurde ein sich aufhebender Offset einmal positiv auf den einen Motortreiber und negativ auf das anderen eingerechnet. So ist es möglich auch starke Lenk-Bewegungen umsetzen zu können ohne das Gleichgewicht des Segways zu stören.
Bei dem neu verwendeten DKS des Segway 2.0 wurde diese Art der Fahrbewegungserzeugung ebenfalls gewählt. Wird ein Hindernis in kritischer Entfernung von den HC-SR04 Sensoren erfasst, wird eine Gegenbewegung durch die Einleitung eines Winkel Offsets in den Regler einberechnet, wodurch das Segway abgebremst wird und eine Kollision vermieden/ oder je nach Geschwindigkeit abgeschwächt wird.
Aus dem Grund, dass die versendeten Reglerparameter im Arduino Programm des Segways bei einem Ausschalten flüchtig sind, werden neu empfangende Reglerwerte in den EEPROM des Ardunio Mega des Segways geschrieben. Ebenfalls ließt das Segway beim Start die letzten Reglerwerte aus dem EEPROM aus und kann so bis zum Einschalten der Fernbedienung sicher stehen.
Neben den neuen Funktionen wurde auch der weiter verwendete Regler überarbeitet. So wurden Wind-Ups durch Begrenzung der Stellgröße für die Motoren und des Summenfehlers implementiert. Hierdurch kann sichergestellt werden, dass das Segway auch nach längeren Phasen mit deutlicher Sollwertabweichung im Fall des Winkels zuverlässig und korrekt geregelt wird. Im Segways 2.0 wird ein Komplementärfilter für die Berechnung der nötigen Werte für die Berechnung der Motortreiber Ansteuerung eingesetzt. Ebenfalls wurde in der Entwicklungszeit die Wert Berechnung durch einen Kalmanfilter implementiert, da in diesem Fall aber keine exaktere Berechnung des Winkels erreicht werden konnte, wurde diese mehr Programmspeicher benötigende Variante nicht weiter eingesetzt.
// Modul : Segway_2.0.c
//
// Datum : 12.11.2017
//
// Funktion : Regelung und Steuerung eines Segways
//
// Implementation : Visual Studio 2012
//
// Autor : Philipp Tewes
//
// Bemerkung : Version 2.2
//
// Letzte Änderung : 18.01.2018
//
//***************************************************************
#include "Wire.h"
#include "SPI.h"
#include "nRF24L01.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "RF24.h"
#include <EEPROM.h>
// Wifi
RF24 radio (40, 53);
const byte rxAddr[6] = "00001";
struct Transmit
{
int dVRx; // -255 - 255
int dVRy; // -255 - 255
bool dSW; // -255 - 255
double P; // klartext
double I; // klartext
double D; // klartext
};
double DrehGeschFB = 0;
double WinkelFahrenFB = 0;
Transmit Nachricht;
bool MotorOff = true;
bool WifiOff = true;
bool TryMe = false;
unsigned long TimeWifiOff = 0;
//LN298N Rechts
byte L298N_23_R = 8; //11; // Pin
byte L298N_14_R = 9; //10; // Pin
byte L298N_EN_R = 5; //13; // Pin
//LN298N Links
byte L298N_23_L = 11; //8; // Pin
byte L298N_14_L = 10; //9; // Pin
byte L298N_EN_L = 13; //5; // Pin
// Variablen für den SR-04
byte SR04_Front_Trigger = 28; // Pin
byte SR04_Front_Echo = 29; // Pin
long SR04_Front_EchoDauer = 0;
long SR04_Front_Entfernung = 0;
byte SR04_Back_Trigger = 4; // Pin
byte SR04_Back_Echo = 3; // Pin
long SR04_Back_EchoDauer = 0;
long SR04_Back_Entfernung = 0;
long SR04_ZeitLetzteFront = 0;
long SR04_ZeitLetzteBack = 0;
long SR04_Intervall = 200;
byte SR04_State = 0;
float SR04_Winkel = 0;
bool SR04_Back_Ausweichen = false;
bool SR04_Front_Ausweichen = false;
// Variablen für das Accelometer und den Gyro
MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;
#define Gryo_Offset 530 // Offset des Gyro
#define Gyro_Verstaerkung 0.00763358 // Verstärkung des Gyro Analog Signal
#define Winkel_offset -1 // Offset des aktuellen Winkels
#define pi 3.14159 // Definition von der zahl pi
// Variablen zum Fahren
float Winkel_offset_Aktiv = 0;
float Motor_offset = 60;
bool Umgefallen = false;
// Variablen für den PID Regeler
struct PID_Werte
{
double kp;
double ki;
double kd;
};
PID_Werte RegelerWerte;
#define Default_PID_P 45
#define Default_PID_I 0.4
#define Default_PID_D 500
// Variablen für die Zeit Berechnung
unsigned long preTime = 0;
float SampleTime = 0.08;
// PWM
double RMotorPower = 0;
double LMotorPower = 0;
double MotorPowerFahrenFB = 0;
void setup() //------------------------------------------------------------------------------------------------------------------------------ Setup
{
Wire.begin(); // Start serieller Kommunikation
accelgyro.initialize(); // Initalisieren des Gyro/Accelometer
pinMode(L298N_23_R, OUTPUT); // Ausgänge zuweisen
pinMode(L298N_14_R, OUTPUT);
pinMode(L298N_23_L, OUTPUT);
pinMode(L298N_14_L, OUTPUT);
pinMode(L298N_EN_R, OUTPUT);
pinMode(L298N_EN_L, OUTPUT);
pinMode(SR04_Front_Trigger, OUTPUT);
pinMode(SR04_Front_Echo, INPUT);
pinMode(SR04_Back_Trigger, OUTPUT);
pinMode(SR04_Back_Echo, INPUT);
radio.begin();
radio.setRetries(15, 15);
radio.setChannel(108);
radio.setDataRate(RF24_250KBPS);
radio.setPALevel(RF24_PA_LOW);
radio.openReadingPipe(0, rxAddr);
radio.startListening();
RegelerWerte = ReadEEPROM(0);
if ((RegelerWerte.kp < 10) || (RegelerWerte.kp > 100))
{
RegelerWerte.kp = Default_PID_P;
}
if ((RegelerWerte.ki < 0) || (RegelerWerte.ki > 2))
{
RegelerWerte.ki = Default_PID_I;
}
if ((RegelerWerte.kd < 150) || (RegelerWerte.kd > 1500))
{
RegelerWerte.kd = Default_PID_D;
}
WriteEEPROM(RegelerWerte, 0);
Serial.begin(115200);
//Serial.println("Start OK");
}
void loop() // ------------------------------------------------------------------------------------------------------------------------------- loop
{
double WinkelKomplement = 0;
double FahrenWinkelFB = 0;
unsigned long SystemZeit = 0;
float dt = 0;
double MotorPower = 0;
while (1)
{
SystemZeit = millis();
dt = ((SystemZeit - preTime) / 1000.0); //vergangene Zeit seit der letzten Berechnung
preTime = SystemZeit;
WinkelKomplement = KomplementFilterFkt(dt);
Wifi(SystemZeit, dt);
if (abs(WinkelKomplement) < 25)
{
if (WifiOff != true)
{
FahrenWinkelFB = FahrenWinkelFBFkt(WinkelKomplement, MotorPower, SystemZeit, dt);
}
else
{
if ((TimeWifiOff + 1500) > SystemZeit)
{
FahrenWinkelFB = WinkelKomplement + 14;
}
else
{
MotorOff = true;
}
}
MotorPower = PID_Fahren(FahrenWinkelFB, SystemZeit, dt);
FahrenMotorFB(MotorPower);
MotorControl();
Umgefallen = false;
}
else
{
Umgefallen = true;
analogWrite(L298N_EN_R, 0);
analogWrite(L298N_EN_L, 0);
}
}
}
double PID_Fahren(double WinkelFahren, unsigned long SystemZeit, float dt) // ---------------------------------------------------------- PID Fahren
{
static double WinkelFahrenD = 0;
static double WinkelFahrenDiff = 0;
static double WinkelFahrenSumme = 0;
static double WinkelFahrenPrevD = 0;
static unsigned long RegelerZeitPrev = 0;
static long ZeitDiff = 0;
static double MotorPower = 0;
ZeitDiff = (SystemZeit - RegelerZeitPrev); //vergangene Zeit seit vor der letzten Berechnung
if (ZeitDiff >= SampleTime)
{
WinkelFahrenD = TiefpassD(WinkelFahren, dt, 0.5);
WinkelFahrenDiff = (WinkelFahrenD - WinkelFahrenPrevD) / ZeitDiff;
WinkelFahrenPrevD = WinkelFahrenD;
WinkelFahrenSumme = WinkelFahrenSumme + (WinkelFahren * ZeitDiff);
if ( WinkelFahrenSumme > 50) // Wind up begrenzen
{
WinkelFahrenSumme = 50;
}
else if (WinkelFahrenSumme < -50)
{
WinkelFahrenSumme = -50;
}
MotorPower = RegelerWerte.kp * WinkelFahren + RegelerWerte.ki * WinkelFahrenSumme + RegelerWerte.kd * WinkelFahrenDiff;
if (MotorPower != 0) // Motor Power begrenzen damit Fahren/Lenken in jeder Situation möglich
{
if ((MotorPower > 0) && (MotorPower + Motor_offset) > 220)
{
MotorPower = 220;
}
else if (MotorPower > 0)
{
MotorPower = MotorPower + Motor_offset;
}
if ((MotorPower < 0) && (MotorPower - Motor_offset) < -220)
{
MotorPower = -220;
}
else if (MotorPower < 0)
{
MotorPower = MotorPower - Motor_offset;
}
}
RegelerZeitPrev = SystemZeit; //Zeit
}
return MotorPower;
}
double FahrenWinkelFBFkt(double WinkelKomplement, double MotorPower, unsigned long SystemZeit, float dt) // ---------------------------FahrenWinkel
{
static byte AktivesFahren = 0;
static double WinkelFahrenAktiv = 0;
static unsigned long FahrtBeginn = 0;
static unsigned long FahrtPause = 0;
static double MotorPowerTP = 0;
static double SR04WinkelKollision = 0;
MotorPowerTP = Tiefpass(MotorPower, dt, 1.2);
if (WinkelFahrenFB < 0) // Vorwärts beschleunigen ------------------------
{
FahrtPause = SystemZeit;
SR04WinkelKollision = DCS(SystemZeit, WinkelKomplement, 0);
if (SR04WinkelKollision == 0)
{
if (((WinkelKomplement < 6) && (AktivesFahren != 1)) || ((FahrtBeginn + 100) > SystemZeit))
{
if (AktivesFahren != 1)
{
FahrtBeginn = SystemZeit;
//Serial.println("Starte vorwärts Fahren !!");
}
//Serial.println("Anfahren vorwärts !!");
AktivesFahren = 1;
WinkelFahrenAktiv = WinkelKomplement + WinkelFahrenFB;
}
else if ((WinkelKomplement < 4) && (AktivesFahren == 1) && ((MotorPowerTP < 80) || (((FahrtBeginn + 300) > SystemZeit) && (MotorPowerTP < 200)))) // Volle fahrt
{
//Serial.println("Vorwärts Fahren voller Winkel !!");
WinkelFahrenAktiv = WinkelKomplement + WinkelFahrenFB;
}
else if ((WinkelKomplement < 5) && (AktivesFahren == 1) && (MotorPowerTP < 80)) // Drosseln wegen Winkel
{
//Serial.println("Winkel zu hoch");
WinkelFahrenFB = (WinkelFahrenFB * abs((((abs(WinkelKomplement) * 20) / 100) - 1)));
WinkelFahrenAktiv = WinkelKomplement - abs(WinkelFahrenFB);
}
else if ((MotorPowerTP >= 70) && (WinkelKomplement < 8) && (AktivesFahren == 1)) // Drossel wegen Motor
{
//Serial.println(" Motor Power zu hoch");
WinkelFahrenFB = (WinkelFahrenFB * abs((((abs(MotorPowerTP) * 1.25) / 100) - 1)));
WinkelFahrenAktiv = WinkelKomplement - abs(WinkelFahrenFB);
}
else if (AktivesFahren == 1) // Notbremsung !!!!!!!
{
//Serial.println("Vorwärts fahrt einbremsung!!!!!!!!!!!!!!!!!!!!!!!!!!");
WinkelFahrenAktiv = WinkelKomplement + 10 ; // Aktiv Gegensteuern
}
}
else
{
WinkelFahrenAktiv = WinkelKomplement + SR04WinkelKollision;
FahrtPause = SystemZeit;
AktivesFahren = 0;
//Serial.println("Fahrt beendet, Kollision vorn!!!!!!!!!!!!");
}
}
else if (WinkelFahrenFB > 0) // Rückwerts bschleunigen --------------------------------------------
{
FahrtPause = SystemZeit;
SR04WinkelKollision = DCS(SystemZeit, WinkelKomplement, 1);
if (SR04WinkelKollision == 0)
{
if (((WinkelKomplement > -4) && (AktivesFahren != 2)) || ((FahrtBeginn + 200) > SystemZeit))
{
if (AktivesFahren != 2)
{
FahrtBeginn = SystemZeit;
//Serial.println("Starte rückwerts Fahren !!");
}
//Serial.println("Anfahren rückwerts !!");
AktivesFahren = 2;
WinkelFahrenAktiv = WinkelKomplement + WinkelFahrenFB;
}
else if ((WinkelKomplement > -3) && (AktivesFahren == 2) && ((MotorPowerTP > -80) || (((FahrtBeginn + 300) > SystemZeit) && (MotorPowerTP > -200))))
{
//Serial.println("Rückwerts Fahren voller Winkel !!");
WinkelFahrenAktiv = WinkelKomplement + WinkelFahrenFB;
}
else if ((WinkelKomplement > -5) && (AktivesFahren == 2) && (MotorPowerTP > -80)) // Drosseln wegen Winkel
{
//Serial.println("Rückwerts Fahren Winkel drosseln");
WinkelFahrenFB = (WinkelFahrenFB * abs((((abs(WinkelKomplement) * 20) / 100) - 1)));
WinkelFahrenAktiv = WinkelKomplement + abs(WinkelFahrenFB);
}
else if ((MotorPowerTP <= -60) && (WinkelKomplement > -8) && (AktivesFahren == 2)) // Drossel wegen Motor
{
//Serial.println("Rückwerts Motor Power zu hoch ");
WinkelFahrenFB = (WinkelFahrenFB * abs((((abs(MotorPowerTP) * 1.25) / 100) - 1)));
WinkelFahrenAktiv = WinkelKomplement + abs(WinkelFahrenFB);
}
else if (AktivesFahren == 2) // Notbremsung !!!!!!!
{
//Serial.println("Rückwerts fahrt zu hoch!!!!!!!!!!!!!!!!!!!!!!!!!!");
WinkelFahrenAktiv = WinkelKomplement - 10 ; // Aktiv Gegensteuern
}
}
else
{
WinkelFahrenAktiv = WinkelKomplement + SR04WinkelKollision;
FahrtPause = SystemZeit;
AktivesFahren = 0;
//Serial.println("Fahrt beendet, Kollision hinten!!!!!!!!!!!!");
}
}
else
{
if ((FahrtPause + 1000) < SystemZeit)
{
FahrtPause = SystemZeit;
AktivesFahren = 0;
//Serial.println("Fahrt beendet!!!!!!!!!!!!");
}
//Serial.println("Prüfe kollision im stand vorn");
SR04WinkelKollision = DCS(SystemZeit, WinkelKomplement, 0);
if (SR04WinkelKollision != 0)
{
//Serial.println("Kollision im Stand vorn");
WinkelFahrenAktiv = WinkelKomplement + SR04WinkelKollision;
}
else
{
//Serial.println("Prüfe kollision im stand hinten");
SR04WinkelKollision = DCS(SystemZeit, WinkelKomplement, 1);
if (SR04WinkelKollision != 0)
{
//Serial.println("Kollision im Stand hinten");
WinkelFahrenAktiv = WinkelKomplement + SR04WinkelKollision;
}
else
{
WinkelFahrenAktiv = WinkelKomplement;
}
}
}
return WinkelFahrenAktiv;
}
double FahrenMotorFB(double MotorPower) //-------------------------------------------------------------------------------------------- Fahren Motor
{
MotorPower = MotorPower + MotorPowerFahrenFB;
RMotorPower = MotorPower + DrehGeschFB;
LMotorPower = MotorPower - DrehGeschFB;
}
void MotorControl() //--------------------------------------------------------------------------------------------------------------- Motor-Control
{
if (LMotorPower < 0)
{
digitalWrite(L298N_23_L, HIGH);
digitalWrite(L298N_14_L, LOW);
}
else if (LMotorPower > 0)
{
digitalWrite(L298N_23_L, LOW);
digitalWrite(L298N_14_L, HIGH);
}
else
{
digitalWrite(L298N_23_L, HIGH);
digitalWrite(L298N_14_L, HIGH);
}
if (RMotorPower > 0)
{
digitalWrite(L298N_23_R, HIGH);
digitalWrite(L298N_14_R, LOW);
}
else if (RMotorPower < 0)
{
digitalWrite(L298N_23_R, LOW);
digitalWrite(L298N_14_R, HIGH);
}
else
{
digitalWrite(L298N_23_R, HIGH);
digitalWrite(L298N_14_R, HIGH);
}
if (MotorOff != true)
{
analogWrite(L298N_EN_L, min(255, abs(LMotorPower * 1.10))); //Geschwindigkeit links
analogWrite(L298N_EN_R, min(255, abs(RMotorPower * 0.90))); //Geschwindigkeit rechts
}
else
{
analogWrite(L298N_EN_L, 0); //Geschwindigkeit links
analogWrite(L298N_EN_R, 0); //Geschwindigkeit rechts
}
}
void Wifi(unsigned long SystemZeit, float dt) //-------------------------------------------------------------------------------------------------------- Wifi
{
if ((radio.available()) && (Umgefallen == false))
{
TryMe = true;
TimeWifiOff = SystemZeit;
radio.read(&Nachricht, sizeof(Nachricht));
DrehGeschFB = -(Nachricht.dVRx / 5); // -255 - 255
WinkelFahrenFB = -(Nachricht.dVRy / 80); // -255 - 255
MotorPowerFahrenFB = -(Nachricht.dVRy / 5); // -255 - 255
MotorPowerFahrenFB = TiefpassFBMotor(MotorPowerFahrenFB, dt, 0.02);
if ((RegelerWerte.kp != Nachricht.P) || (RegelerWerte.ki != Nachricht.I) || (RegelerWerte.kd != Nachricht.D))
{
RegelerWerte.kp = Nachricht.P; // klartext
RegelerWerte.ki = Nachricht.I; // klartext
RegelerWerte.kd = Nachricht.D; // klartext
WriteEEPROM(RegelerWerte, 0);
}
if ((WifiOff == true) && (TryMe == true) && (Nachricht.dVRx == 0) && (Nachricht.dVRy == 0))
{
// Warten auf Aktion der FB
}
else if (WifiOff == true)
{
MotorOff = false;
TryMe = false;
WifiOff = false;
}
}
else
{
if (((TimeWifiOff + 500) < SystemZeit) || (Umgefallen == true))
{
TryMe = false;
WifiOff = true;
}
}
}
double DCS(unsigned long SystemZeit, double Winkel, bool Sensor) // -------------------------------------------------------------------- DCS
{
static double KollisionWinkelFront = 0;
static double KollisionWinkelBack = 0;
if (((SystemZeit - SR04_ZeitLetzteFront) > SR04_Intervall) || ((SystemZeit - SR04_ZeitLetzteBack) > SR04_Intervall))
{
if (Sensor == 0)
{
SR04_ZeitLetzteFront = SystemZeit;
return (KollisionWinkelFront = DCSfront(Winkel));
}
else
{
SR04_ZeitLetzteBack = SystemZeit;
return (KollisionWinkelBack = DCSback(Winkel));
}
}
else if (Sensor == 0)
{
return KollisionWinkelFront;
}
else
{
return KollisionWinkelBack;
}
}
double DCSfront(double Winkel)
{
digitalWrite(SR04_Front_Trigger, HIGH);
delayMicroseconds(10);
digitalWrite(SR04_Front_Trigger, LOW);
SR04_Front_EchoDauer = pulseIn(SR04_Front_Echo, HIGH, 4500);
SR04_Front_Entfernung = (SR04_Front_EchoDauer / 2) * 0.03432;
if ((SR04_Front_Entfernung > 2) && (SR04_Front_Entfernung < 10))
{
//Serial.println("Kollision vorn Ausweichen");
SR04_Front_Ausweichen = true;
SR04_Back_Ausweichen = false;
return 5;
}
else
{
if (SR04_Front_Ausweichen == true)
{
if (Winkel < -7)
{
//Serial.println("Kollision vorn Gegensteuern");
return -10;
}
//Serial.println("Vorn Gegensteuern beendet");
SR04_Front_Ausweichen = false;
return 0;
}
//Serial.println("Vorn keine Kollision");
return 0;
}
}
double DCSback(double Winkel)
{
digitalWrite(SR04_Back_Trigger, HIGH);
delayMicroseconds(10);
digitalWrite(SR04_Back_Trigger, LOW);
SR04_Back_EchoDauer = pulseIn(SR04_Back_Echo, HIGH, 4500);
SR04_Back_Entfernung = (SR04_Back_EchoDauer / 2) * 0.03432;
if ((SR04_Back_Entfernung > 2) && (SR04_Back_Entfernung < 10))
{
//Serial.println("Kollision hinten!!!!");
SR04_Back_Ausweichen = true;
SR04_Front_Ausweichen = false;
return -5;
}
else
{
if (SR04_Back_Ausweichen == true)
{
if (Winkel > 5)
{
//Serial.println("Kollision hinten Gegensteuern");
return 10;
}
//Serial.println("Hinten Gegensteuern beendet");
SR04_Back_Ausweichen = false;
return 0;
}
//Serial.println("Hinten keine Kollision");
return 0;
}
}
double KomplementFilterFkt(float dt) // ------------------------------------------------------------------------------------------------ Komplementär Filter
{
static double WinkelRohwert = 0;
static double GyroRohwert = 0;
static double KomplementWinkel = 0;
static float K = 1.2;
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
WinkelRohwert = (atan2(ay, az) * 180 / pi + Winkel_offset);
GyroRohwert = (double)gx * Gyro_Verstaerkung;
float A = K / (K + dt);
KomplementWinkel = (A * (KomplementWinkel + GyroRohwert * dt) + (1 - A) * WinkelRohwert); //Winkel Berechnung
return KomplementWinkel;
}
double Tiefpass(double Eingang, double dt, double k) // -------------------------------------------------------------------------------------------- Tiefpass
{
static double prevMotorTP = 0;
double alphaTP = k / (k + dt);
double MotorTP = alphaTP * prevMotorTP + (1 - alphaTP) * Eingang;
prevMotorTP = MotorTP;
return MotorTP;
}
double TiefpassFBMotor(double Eingang, double dt, double k) // ------------------------------------------------------------------------------------- Tiefpass
{
static double prevMotorFBTP = 0;
double alphaTP = k / (k + dt);
double MotorFBTP = alphaTP * prevMotorFBTP + (1 - alphaTP) * Eingang;
prevMotorFBTP = MotorFBTP;
return MotorFBTP;
}
double TiefpassD(double Eingang, double dt, double k) // ------------------------------------------------------------------------------------- Tiefpass
{
static double prevD = 0;
double alphaTP = k / (k + dt);
double D = alphaTP * prevD + (1 - alphaTP) * Eingang;
prevD = D;
return D;
}
PID_Werte ReadEEPROM(byte Adresse) // ------------------------------------------------------------------------------------------------------------------ EEPROM
{
PID_Werte TmpLesen;
for (unsigned int t = 0; t < sizeof(TmpLesen); t++)
{
*((char*)&TmpLesen + t) = EEPROM.read(Adresse + t);
}
return TmpLesen;
}
void WriteEEPROM(PID_Werte SpWerte, byte Adresse)
{
for (unsigned int t = 0; t < sizeof(SpWerte); t++)
{
EEPROM.write(Adresse + t, *((char*)&SpWerte + t));
}
}
Filterung der Sensorwerte
Die Messwerte des MPU6050 unterliegen einem Rauschen welches unterdrückt werden musste. Desweiteren besitzt das Gyroskop einen drift welcher ebenfalls unterdrückt werden muss. Wie im Kapitel Software ersichtlich wurde dies mit einem Komplementärfilter umgesetzt. Hierdurch wird neben der Rauschunterdrückung der Drift des Gyroskop unterdrückt. Neben der Variante mit Komplementärfilter wurde die Lösung dieser beiden Probleme in der Entwicklungszeit ebenfalls mit einem Kalmanfilter realisiert. Beide Lösungen brachten dabei gleich gute Ergebnisse. Aus dem Grund, dass ein Komplementärfilter aber weniger Code beansprucht, wurde sich für diese Lösung entschieden. Der entwickelte Code der Lösung mit einem Kalmanfilter ist dabei im folgenden Code:
// Modul : KalmanFilter.h
//
// Datum : 01.11.2017
//
// Funktion : Kalmanfilter für ein Gyroskop und einen Beschleunigungssensor, berechnet wird der resultierende Winkel
//
// Implementation : Visual Studio 2012
//
// Autor : Philipp Tewes
//
// Bemerkung : Version 1.2
//
// Letzte Änderung : 03.12.2017
//
//***************************************************************
#ifndef _KalmanFilter_h
#define _KalmanFilter_h
static double P[2][2];
static double K[2];
static double dtWinkel;
static double S;
static double QWinkel;
static double QGierrate;
static double RMessung;
static double Winkel;
static double Gierrate;
static double dtGierrate;
void iniKalmanfilter()
{
QWinkel = 0.001; // Systemunsicherheit
QGierrate = 0.003; // System faktor
R_Messung = 0.03; // Messunsicherheit
Winkel = 0; // Winkel zurücksetzen
Gierrate = 0; // Faktor zurücksetzen
P[0][0] = 0; // Kovarianzmatrix, einmal mit SetWinkel definieren
P[0][1] = 0;
P[1][0] = 0;
P[1][1] = 0;
}
double getWinkel(double NeuerWinkel, double NeueGierrate, double dt) // Alles in Si eingeben
{
// Schätzen
dtGierrate = (NeueGierrate - Gierrate);
Winkel = (Winkel + (dt * dtGierrate));
P[0][0] = (P[0][0] + (dt * (dt * P[1][1] - P[0][1] - P[1][0] + QWinkel)));
P[0][1] = (P[0][1] - (dt * P[1][1]));
P[1][0] = (P[1][0] - (dt * P[1][1]));
P[1][1] = (P[1][1] + (QGierrate * dt));
// Update
S = (P[0][0] + RMessung);
K[0] = (P[0][0] / S);
K[1] = (P[1][0] / S);
dtWinkel = (NeuerWinkel - Winkel);
Winkel = (Winkel + (K[0] * dtWinkel));
Gierrate = (Gierrate + (K[1] * dtWinkel));
P[0][0] = (P[0][0] - (K[0] * P[0][0]));
P[0][1] = (P[0][1] - (K[0] * P[0][1]));
P[1][0] = (P[1][0] - (K[1] * P[0][0]));
P[1][1] = (P[1][1] - (K[1] * P[0][1]));
return Winkel;
}
void setQWinkel(double NeueQWinkel)
{
QWinkel = NeueQWinkel;
}
void setQGierrate(double NeueQGierrate)
{
QGierrate = NeueQGierrate;
}
void setRMessung(double NeuRMessung)
{
RMessung = NeuRMessung;
}
double getQWinkel()
{
return QWinkel;
}
void setWinkel(double NeuerWinkel)
{
Winkel = NeuerWinkel;
}
double getdtGierrate()
{
return dtGierrate;
}
#endif
Teile-Liste
- 3x 25cm x 13,5cm Plexiglasscheibe
- 2x RC Reifen 1:8 (Durchmesser 10cm)
- 1x Holzstab 100cm x 1,8cm
- 1x Arduino Mega 2560
- 1x RF24 Wifi Modul
- 2x HC-SR04 Ultraschallsensor
- 2x Motor Drive L298N
- 1x Beschleunigungs- und Gyro Sensor MPU-6050
- 2x Getriebemotor 12V DC MFA 919D1481 50:1
- 1x Ladegerät ACS110
- 1x Akku-Pack PP2
- 2x Achskupplung (M5 auf M5)
- 2x Gummi Unterlegscheibe (Innendurchmesser 0,4cm Außendurchmesser 1,7cm)
- 6x Metall Unterlegscheibe (Innendurchmesser 0,8cm Außendurchmesser 2,3cm)
- 2x Metallschrauben M5 Länge 3cm
- 48x M5 Unterlegscheiben
- 64x M5 Mutter
- 1x M5 Gewindestange 1m
- 2x Tamya Stecker mit 5cm Kabel
- 14x M3 Schrauben (1,5cm) mit Muttern
FernDuino
Der Name "FernDuino" ist ein Wortspiel und soll die beiden Hauptmerkmale "Fernbedienung" und "Arduino" des fertigen Aufbaus miteinander in Bezug bringen. Der FernDuino soll das bestehende Konzept des Segways aufgreifen und dieses um eine nützliche Komponente - der Fernsteuerung - erweitern. Die Bewegungssteuerung des Segways ist unter anderem mit dem linken Joystick oder alternativ per Neigungswinkeländerung des FernDuinos möglich. Zudem können die Reglerwerte des Segways on the fly mit dem rechten analogen Joystick verändert und korrigiert werden. Bei einem Neustart des FernDuinos sind die letzten Reglerwerte direkt im EEPROM gespeichert und das Segway balanciert sich aus. Ein 4-Zeilen LCD Modul visualisiert die einzelnen Reglerparameter und die gewählte Bewegungssteuerungsart. Der festintegrierte NiMH Akku kann ganz einfach mit dem 2-poligen Tamiyastecker aufgeladen werden.
Zusammenfassend sind die Hauptmerkmale des FernDuinos:
- Funkverbindung zwischen dem FernDuino und dem Segway
- Bewegungs- und Reglerparametersteuerung mit den analogen Joysticks
- Bewegungssteuerung per Neigungswinkeländerung
- Visualisierung der Reglerparameter und Bewegungssteuerungsart
- EEPROM Speicher für die Reglerparameter
- Einfaches Aufladen des Akkus per Tamiyastecker
Konzept und einzelne Module
Die nachfolgenden Module wurden aufgrund einiger zuvor aufgestellten Kriterien ausgesucht und bestellt.
- Kriterium 1: Funktionalität
Um den FernDuino zu einem Highlight zu machen wurden einige Konzepte mit möglichen Optionen und Vorschlägen zusammengestellt. So sollte sich der FernDuino kaum von einer handelsüblichen käuflichen Fernbedienung/Controller unterscheiden, aber dennoch einige herausragende Eigenschaften bieten. Aus diesem Grund fiel die Wahl auf ein offenes Funkprotokoll, analogen Joysticks, Gyrosensor und einem LCD Modul.
- Kriterium 2: Bibliotheken
Die Wahl des Einplatinenrechners fiel schnell auf einen aus der Familie Arduino, da ein Arduino schon erfolgreich im Segway eingesetzt wurde. Um nicht komplett in die Registerebene einzutauchen, sollten die Module schon fertige Bibliotheken entweder vom Hersteller oder von anderen Autoren bereitstellen.
- Kriterium 3: Bauform
Der FernDuino sollte gut in der Hand liegen und somit musste ein Kompromiss zwischen Funktionalität und Bauform getroffen werden. Einige Module hatten gute Funktionalitäten, wurden aber aufgrund der Bauform gegen andere ersetzt.
- Kriterium 4: Kosten
Da die Kosten des Segways schon den Budgetrahmen gesprengt hatten, sollte der FernDuino mit den einzelnen Modulen möglichst kostengünstig aufgebaut werden.
Einplatinenrechner
Das Herzstück des FernDuinos basiert auf einem Arduino Nano. Dieser Arduino Nano besitzt nahezu dieselben Funktionalitäten und Leistungen wie der Arduino Uno und zeichnet sich zudem durch seine relativ kompakte Bauform aus. Um zusätzliche Module anschließen zu können stehen insgesamt 14 I/O Pins, wovon 8 analoge Eingänge sind, zur Verfügung. Der Einplatinenrechner verfügt zudem über einen 5V und 3.3V Spannungsausgang.
- Mikrocontroller: ATmega328
- Flash-Speicher: 32KB
- SRAM: 2KB
- EEPROM: 1KB
- Taktrate: 16MHz
- I/O-Pins: 14, davon 6 PWM und 8 analoge Eingänge
- Anschluss: Mini-USB
- LEDs: RXD, TXD, Power, Pin 13
- Größe: ca. 1,85cm x 4,3cm
- Betriebsspannung: 5V
- Empfohlene Eingangsspannung: 7-12V
- Maximale Eingangsspannung: 20V
- Maximaler Strom pro I/O-Pin: 40mA
- Belastbarkeit des 3.3V Ausgangs: 50mA
Aufgrund dieser Eigenschaften und den technischen Daten wurde der Arduino Nano für das Projekt gewählt.
Funkmodul
Als Funkmodul wurde ein nRF24L01+ single Chip Transceiver von Nordic Semiconductor gewählt, somit kann dieser Chip als Sender und Empfänger eingesetzt werden. Die Transceiver funken im 2.4GHz Netz und ermöglichen eine separate Kanaleinstellung (bis zu 128 Kanäle) um mögliche Funkkollisionen mit anderen Netzwerken zu vermeiden. Die Kommunikation zu dem Arduino Nano geschieht über den SPI Bus. Erhältlich sind die Funkmodule in zwei verschiedenen Versionen: mit integrierter Antenne oder mit einem SMA Antennenanschluss. Bevor die einzelnen Module gekauft wurden, wurden einige Tests mit vorab bestellten Prototypen durchgeführt. Die Versionen mit integrierter Antenne haben untereinander nur eine sehr geringe Reichweite (unter 5 Meter) und waren somit für den Einsatz im Segway und des FernDuinos unzureichend. Eine Verbesserung der Reichweite zeigte sich indem ein Funkmodul mit SMA Antenne verwendet und ein Elko Kondensator zwischen 3.3V und GND auf dem Chip angebracht wurde. Durch diese Konstellation (Mastermodul mit SMA Antenne, Slavemodul mit integrierter Antenne und beide jeweils mit einem Kondensator) sind Reichweiten über 10 Meter möglich und ideal für die Segway Anwendung.
- Betriebsspannung: 1.9-3.6V
- Frequenzband: 2.4GHz
- Datenrate: 250kbps, 1Mbps und 2Mbps
- 128 Kanäle
- 900nA PowerDown Modus und 26uA im Standby
- Interface: SPI Bus
- Bauform mit integrierter Antenne oder mit mit SMA Anschluss
Steuerung
Für die Steuerung wurden insgesamt zwei analoge Joysticks verbaut. Diese benötigen jeweils eine Eingangsspannung von 5V, einen digitalen Pin für die Tastenfunktion und zwei analoge Eingänge am Arduino um die x- und y-Werte zu übertragen. Der linke Joystick ist für das Fahren und der rechte für die ein- und verstellbaren Reglerwerte des FernDuinos verantwortlich.
- Betriebsspannung: 3.3-5V
- 2 analoge Ausgänge (10k Potentiometer für x und y-Koordinaten)
- Digitaler Ausgang für Tastendruck
Als Besonderheit wurde zudem ein 3-Achsen-Lage- und Beschleunigungssensor, der MPU6050, für zusätzlichen Fahrspaß verbaut. Durch einen Tastendruck auf den linken Joystick wird der MPU6050 aktiviert und das Segway kann durch die Veränderung der Neigungswinkel des FernDuinos gesteuert werden. Der MPU6050 wird ebenfalls mit 5V versorgt und kommuniziert per I2C mit dem Arduino.
Display
Das FernDuino Menü wird über ein LCD Modul mit der Bezeichnung 2004 dargestellt. Es verfügt über insgesamt 4 Zeilen mit jeweils 20 Zeichen und kann per I2C angesprochen werden, somit werden nur 5V, GND und die beiden I2C Pins (SDA und SCL) benötigt. Die Schriftfarbe ist weiss auf einem blauen Hintergrund und per Jumper auf der Platinenrückseite kann die Hintergrundbeleuchtung an- und ausgeschaltet werden.
- Betriebsspannung: 5V
- 4 Zeilen mit 20 Zeichen
- Interface: I2C
- Weiss auf blauem Hintergrund
Akku
In dem FernDuino wurde ein NiMH-Akku mit einer Nennspannung von 7.2V und einer Kapazität von 1100mAh fest verbaut. Der 2-polige Tamiyastecker wurde nach außen geführt, sodass eine Aufladung des Akkus per Ladegerät kein Problem darstellt.
- Technologie: NiMH
- Nennspannung: 7.2V
- Kapazität: 1100mAh
- schnellladefähig
Wichtig: Um mögliche Schäden des FernDuinos zu vermeiden, sollte der Akku nur im ausgeschalteten Zustand des FernDuinos aufgeladen werden!
Teile-Liste
- 17x 10mm M2.5 Schrauben
- 21x M2.5 Unterlegscheiben
- 16x M2.5 Muttern
- 16x 16mm M2.5 Abstandsbolzen
- 6x 20mm M2.5 Abstandsbolzen
- 6mm, 9mm und 12mm Beschriftungsband
- 4x Kabelbinder
- Schrumpfschlauch
- 1,5mm² rote und schwarze Leitungen
- 0,25mm² Litze
- Lötzinn
- 2x analoge Joysticks
- 1x Schalter
- 1x nRF24L01+ Modul mit SMA Antenne
- 1x Arduino Nano
- 1x MPU6050
- 1x LCD 2004 I2C
- 1x 10uF 50V
- 1x NiMH Akku 7.2V, 1100mAh
- 1x Lochrasterplatine 50x70mm
- 1x Polystyrol 250x500mm und 4mm Stärke
- 1x silberne Lackierfarbe
Technischer Aufbau
Der technische Aufbau wurde mit der freien Software Fritzing erstellt. Hier stand vor allem die Entwicklung des Schaltplans im Vordergrund. Die verschiedenen einzelnen Module konnten aus der Fritzing Datenbank per Drag and Drop auf die Arbeitsmappe gezogen werden.
Die einzelnen Signalleitungen und Spannungsversorgungen zwischen den Modulen und dem Arduino Nano wurden mit 0,25mm² Litze angefertigt. Die NiMH Akku Spannungsversorgung wurde mit 1,5mm² an den Arduino gelegt.
Mechanischer Aufbau
Wie auch schon bei dem Segway wurde sich für ein Konzept mit mehreren Ebenen entschieden, sodass die Länge und Breite des FernDuinos relativ klein wurde und der Aufbau mehr in die Höhe ging.
Die erste Etage ist das Grundgerüst des FernDuinos. Hier werden die einzelnen Löcher für die Abstandsbolzen angefertigt und eingesetzt. Ebenfalls wird der Akku fest in die Konstruktion verkeilt. Links und rechts werden die analogen Joysticks mit den dazugehörigen Muttern angebracht.
Auf der zweiten Etage ist die Lochrasterplatine mit dem Arduino angebracht. Diese befindet sich direkt oberhalb des Akkus mit einem gewissen Abstand dazwischen, sodass die Litzen gut unterhalb und oberhalb der Platine angebracht werden können.
Das LCD Modul wird auf der dritten Etage ebenfalls mit einem gewissen Höhenunterschied platziert.
Etage 1
- Polystyrol Grundfläche mit den gegebenen Maßen ausschneiden. Bei Bedarf lackieren.
- Ø2.7mm Bohrer für die Bohrlöcher
- 16mm Abstandsbolzen für die Joysticks und die Lochrasterplatine mit dem Arduino setzen
- 20mm Abstandsbolzen für das LCD und die Akkuhalterung platzieren
- Abstandsbolzen mit den M2.5 Schrauben und Unterlegscheiben verschrauben
- Akku waagerecht einsetzen und verkeilen
- Analoge Joysticks mit M2.5 Muttern links und rechts anbringen
- Beschriftungen "Drive" und "Regler" mit einem 12mm Beschriftungsband anfertigen und platzieren
Etage 2
- 20mm Abstandsbolzen für die Lochrasterplatine mit dem Arduino setzen
- 20mm Abstandsbolzen für Akkuhalterung platzieren
- Lochrasterplatine mit Muttern verschrauen
- 16mm Abstandsbolzen für das LCD platzieren
Etage 3
- 16mm Abstandsbolzen für das LCD platzieren
- LCD Modul mit Unterlegscheiben und Muttern befestigen
- Beschriftungen "ON / OFF" und "ANT" mit 6mm Beschriftungsband befestigen
- Beschriftungen "FernDuino" und "V1.0" mit 9mm Beschriftungsband befestigen
Software
Die Software des FernDuinos wurde mit der Entwicklungsumgebung ARDUINO (Version 1.8.5) erstellt. Für die korrekten Funktionen der einzelnen Module wurden separate Bibliotheken von verschiedenen Autoren benutzt, die sie veröffentlicht haben. Der gesamte nachfolgende Quellcode des FernDuinos mit den Bibliotheken befindet sich ebenfalls im SVN Ordner.
//FernDuino V1.1
//Autor: Sven Gaida, Philipp Tewes
//Letzte Änderung: 18.01.2018
//########### Bibliotheken ###########
#include <nRF24L01.h>
#include <RF24.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <SPI.h>
#include <I2Cdev.h>
#include <MPU6050.h>
#include <EEPROM.h>
//########### Defines ###########
#define Default_PID_P 45
#define Default_PID_I 0.4
#define Default_PID_D 500
//########### Konstanten ###########
//NRF24L01
const int CE_PIN = 7;
const int CSN_PIN = 8;
//Joystick Drive
const int dVRx_PIN = A7;
const int dVRy_PIN = A6;
//Interrupt Int1
const int dSW_PIN = 2;
//Joystick Regler
const int rVRx_PIN = A3;
const int rVRy_PIN = A2;
//Interrupt Int1
const int rSW_PIN = 3;
//LC Display
const int Zeilen = 4;
const int Zeichen = 20;
//########### Objekte ###########
//LC Display
LiquidCrystal_I2C lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
//NRF24L01
RF24 radio(CE_PIN, CSN_PIN);
//Accelometer u. Gyro
MPU6050 accelgyro;
struct Transmit
{
int dVRx;
int dVRy;
bool dSW;
double P;
double I;
double D;
}Daten;
int rEingabe = 0;
//########### Variablen ###########
//NRF24L01
const byte rxAddr[6] = "00001";
//Accelometer u. Gyro
int16_t ax, ay, az;
int16_t gx, gy, gz;
double Bewegung_y,Bewegung_x;
unsigned long timeNow;
unsigned long started_waiting_at;
boolean timeout;
//Interrupt
volatile int rButton = 0, dButton = 0;
volatile unsigned long alteZeit=0, entprellZeit=20;
void setup()
{
Serial.begin(115200);
// EEPROM
Daten = ReadEEPROM(0);
if ((Daten.P < 10) || (Daten.P > 100))
{
Daten.P = Default_PID_P;
}
if ((Daten.I < 0) || (Daten.I > 2))
{
Daten.I = Default_PID_I;
}
if ((Daten.D < 150) || (Daten.D > 1500))
{
Daten.D = Default_PID_D;
}
WriteEEPROM(Daten, 0);
//LC Display
lcd.begin(Zeichen,Zeilen);
//Joystick Drive
pinMode(dSW_PIN,INPUT);
digitalWrite(dSW_PIN,HIGH);
//Joystick Regler
pinMode(rSW_PIN,INPUT);
digitalWrite(rSW_PIN,HIGH);
//Interrupt
attachInterrupt(digitalPinToInterrupt(dSW_PIN),drive_ISR,LOW);
attachInterrupt(digitalPinToInterrupt(rSW_PIN),regler_ISR,LOW);
//NRF24L01
radio.begin();
radio.setRetries(15,15);
radio.setChannel(108);
radio.setDataRate(RF24_250KBPS);
//RF24_PA_MIN, RF24_PA_LOW, RF24_PA_HIGH and RF24_PA_MAX
radio.setPALevel(RF24_PA_LOW);
radio.openWritingPipe(rxAddr);
radio.stopListening();
//Gyro
accelgyro.initialize();
lcd.setCursor(0,1);
lcd.print(" Segway 2.0");
delay(700);
lcd.setCursor(0,3);
lcd.print(" with Fernduino 1.0");
delay(1500);
lcd.clear();
lcd.setCursor(0,1);
lcd.print(" by Sven Gaida");
delay(700);
lcd.setCursor(0,3);
lcd.print(" and Philipp Tewes");
delay(1500);
lcd.clear();
lcd.setCursor(0,1);
lcd.print("To activate the");
lcd.setCursor(0,2);
lcd.print("Segway touch Drive");
lcd.setCursor(0,3);
lcd.print("Joystick on the left");
delay(4000);
lcd.clear();
lcd.setCursor(0,1);
lcd.print(" Have fun");
lcd.setCursor(0,3);
lcd.print(" and take care");
delay(1500);
lcd.clear();
//Menü
lcd.setCursor(1,1);
lcd.print("P-Anteil:");
lcd.setCursor(17,1);
lcd.print("0");
lcd.setCursor(18,1);
lcd.print(Daten.P,0);
lcd.setCursor(1,2);
lcd.print("I-Anteil:");
lcd.setCursor(16,2);
lcd.print(Daten.I);
lcd.setCursor(1,3);
lcd.print("D-Anteil:");
lcd.setCursor(16,3);
lcd.print("0");
lcd.setCursor(17,3);
lcd.print(Daten.D,0);
}
void loop()
{
switch(rButton)
{
case 0:
lcd.setCursor(0,3);
lcd.print(" ");
if(dButton == 0)
{
lcd.setCursor(0,0);
lcd.print("Joystick Steuerung ");
Daten.dVRx = map(analogRead(dVRx_PIN),0,1023,-255,255);
Daten.dVRy = map(analogRead(dVRy_PIN),0,1023,-255,255);
//Invertiere Eingabe
Daten.dVRy = Daten.dVRy * (-1);
//Offset x
if(Daten.dVRx <= 15 && Daten.dVRx >= -15)
{
Daten.dVRx = 0;
}
//Offset y
if(Daten.dVRy <= 15 && Daten.dVRy >= -15)
{
Daten.dVRy = 0;
}
}
if(dButton == 1)
{
lcd.setCursor(0,0);
lcd.print("Gyroscope Sensor ");
accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);
Bewegung_y = (atan2(ay, az) * 180 / 3.14);
Bewegung_x = (atan2(ax, az) * 180 / 3.14);
//Grenzbereiche
if(Bewegung_x > 20)
{
Bewegung_x = 20;
}
if(Bewegung_x < -20)
{
Bewegung_x = -20;
}
//Offset
if(Bewegung_x <= 2 && Bewegung_x >= -2)
{
Bewegung_x = 0;
}
Daten.dVRx = map(Bewegung_x,-20,20,-255,255);
//Grenzbereiche
if(Bewegung_y > 20)
{
Bewegung_y = 20;
}
if(Bewegung_y < -20)
{
Bewegung_y = -20;
}
//Offset
if(Bewegung_y <= 2 && Bewegung_y >= -2)
{
Bewegung_y = 0;
}
Daten.dVRy = map(Bewegung_y,-20,20,-255,255);
}
//Kontrolle und übertrage
radio.write(&Daten,sizeof(Daten));
// Serial.print("Joystickwerte");
// Serial.print("\n");
// Serial.print(Daten.dVRx);
// Serial.print("\n");
// Serial.print(Daten.dVRy);
// Serial.print("\n");
// Serial.print("Taste=");
// Serial.print(Daten.dSW);
// Serial.print("\n");
// Serial.print("P=");
// Serial.print(Daten.P);
// Serial.print("\n");
// Serial.print("I=");
// Serial.print(Daten.I);
// Serial.print("\n");
// Serial.print("D=");
// Serial.print(Daten.D);
// Serial.print("\n");
delay(100);
break;
//Menü Regler
case 1:
lcd.setCursor(0,0);
lcd.print("Reglereinstellungen ");
lcd.setCursor(0,1);
lcd.print(">");
lcd.setCursor(19,1);
rEingabe = map(analogRead(rVRx_PIN), 0, 1023, -100, 100);
//Empfindlichkeit Joystick
if(rEingabe > 5 && rEingabe <= 98)
{
Daten.P++;
delay(300);
}
if(rEingabe > 98)
{
Daten.P = Daten.P + 10;
delay(600);
}
if(rEingabe < -5 && rEingabe >= -98)
{
Daten.P--;
delay(300);
}
if(rEingabe < -98)
{
Daten.P = Daten.P - 10;
delay(600);
}
//Wertebereiche
if(Daten.P <= 0)
{
Daten.P = 0;
}
if(Daten.P >= 200)
{
Daten.P = 200;
}
//Cursor setzen
if(Daten.P < 10)
{
lcd.setCursor(17,1);
lcd.print("00");
lcd.setCursor(19,1);
lcd.print(Daten.P,0);
}
if(Daten.P < 100 && Daten.P >=10)
{
lcd.setCursor(17,1);
lcd.print("0");
lcd.setCursor(18,1);
lcd.print(Daten.P,0);
}
if(Daten.P < 1000 && Daten.P >=100)
{
lcd.setCursor(17,1);
lcd.print(Daten.P,0);
}
WriteEEPROM(Daten,0);
break;
case 2:
lcd.setCursor(0,1);
lcd.print(" ");
lcd.setCursor(0,2);
lcd.print(">");
lcd.setCursor(16,2);
rEingabe = map(analogRead(rVRx_PIN), 0, 1023, -100, 100);
if(rEingabe > 5 && rEingabe <= 98)
{
Daten.I = Daten.I + 0.01;
delay(300);
}
if(rEingabe > 98)
{
Daten.I = Daten.I + 0.1;
delay(600);
}
if(rEingabe < -5 && rEingabe >= -98)
{
Daten.I = Daten.I - 0.01;
delay(300);
}
if(rEingabe < -98)
{
Daten.I = Daten.I - 0.1;
delay(600);
}
if(Daten.I <= 0.0)
{
Daten.I = 0.0;
}
if(Daten.I >= 0.99)
{
Daten.I = 0.99;
}
lcd.print(Daten.I);
WriteEEPROM(Daten,0);
break;
case 3:
lcd.setCursor(0,2);
lcd.print(" ");
lcd.setCursor(0,3);
lcd.print(">");
lcd.setCursor(19,3);
rEingabe = map(analogRead(rVRx_PIN), 0, 1023, -100, 100);
if(rEingabe > 5 && rEingabe <= 98)
{
Daten.D++;
delay(300);
}
if(rEingabe > 98)
{
Daten.D = Daten.D + 10;
delay(600);
}
if(rEingabe < -5 && rEingabe >= -98)
{
Daten.D--;
delay(300);
}
if(rEingabe < -98)
{
Daten.D = Daten.D - 10;
delay(600);
}
if(Daten.D <= 200)
{
Daten.D = 200;
}
if(Daten.D >= 2000)
{
Daten.D = 2000;
}
if(Daten.D < 1000)
{
lcd.setCursor(16,3);
lcd.print("0");
lcd.setCursor(17,3);
lcd.print(Daten.D,0);
}
if(Daten.D >= 1000)
{
lcd.setCursor(16,3);
lcd.print(Daten.D,0);
}
WriteEEPROM(Daten,0);
break;
}
}
//Tastendruck
void drive_ISR()
{
//Tasten entprellen
if((millis() - alteZeit) > entprellZeit)
{
dButton++;
alteZeit = millis();
}
if(dButton == 2)
{
dButton=0;
}
}
void regler_ISR()
{
//Tasten entprellen
if((millis() - alteZeit) > entprellZeit)
{
rButton++;
alteZeit = millis();
}
if(rButton == 4)
{
rButton=0;
}
}
Transmit ReadEEPROM(byte Adresse)
{
Transmit TmpLesen;
for (unsigned int t=0; t<sizeof(TmpLesen); t++)
{
*((char*)&TmpLesen + t) = EEPROM.read(Adresse + t);
}
return TmpLesen;
}
void WriteEEPROM(Transmit SpWerte, byte Adresse)
{
for (unsigned int t = 0; t < sizeof(SpWerte); t++)
{
EEPROM.write(Adresse + t, *((char*)&SpWerte + t));
}
}
Der nachfolgende Programmcode kann bei Problemen mit der Funkkommunikation ausprobiert werden. Hierzu wird ein zweiter Arduino (Empfänger) mit einem nRF24L01+ Modul wie in dem Schaltungsaufbau verkabelt und per Seriellen Monitor ausgewertet. Bei erfolgreicher Kommunikation können die analogen Joystickwerte und Reglerparameter des Senders angezeigt werden.
//Receive V1.1
//Autor: Sven Gaida
//Letzte Änderung: 21.01.2018
#include <SPI.h>
#include <nRF24L01.h>
#include "RF24.h"
struct Transmit
{
int dVRx;
int dVRy;
bool dSW;
double P;
double I;
double D;
}Daten;
RF24 radio (7, 8);
const byte rxAddr[6] = "00001";
void setup()
{
Serial.begin(115200);
radio.begin();
radio.setChannel(108);
radio.setDataRate(RF24_250KBPS);
radio.setPALevel(RF24_PA_LOW);
radio.openReadingPipe(0, rxAddr);
radio.startListening();
}
void loop()
{
if ( radio.available())
{
radio.read(&Daten,sizeof(Daten));
Serial.print("x=");
Serial.print(Daten.dVRx);
Serial.print("\n");
Serial.print("y=");
Serial.print(Daten.dVRy);
Serial.print("\n");
Serial.print("Taste=");
Serial.print(Daten.dSW);
Serial.print("\n");
Serial.print("P=");
Serial.print(Daten.P);
Serial.print("\n");
Serial.print("I=");
Serial.print(Daten.I);
Serial.print("\n");
Serial.print("D=");
Serial.print(Daten.D);
Serial.print("\n");
}
}
Bedienungsanleitung und Menüführung
Der folgende Abschnitt soll die Menüführung des FernDuinos in Kurzform erklären:
Steuerung mit dem Joystick
Nachdem starten des Fernduinos durch den Ein- und Ausschalter an der Gehäusevorderseite, befindet sich der Benutzer zunächst in dem Menüpunkt der analogen Joysticksteuerung. Dieses wird dem Benutzer durch die erste Zeile des LCDs ersichtlich gemacht. Zudem werden dem Benutzer die eingestellten Reglereinstellungen des Segways jederzeit angezeigt. Im Hintergrund sucht sich der FernDuino eigenständig das Segway um eine Funkkommunikation zu starten. Aus diesem Grund sollte sich das Segway beim Startvorgang in der Nähe des FernDuinos befinden. Nach sehr kurzer Zeit kann das Segway mit dem linken analogen Joystick gesteuert werden.
Steuerung mit dem Gyroscope
Durch ein Drücken des linken analogen Joysticks wird die Segwaysteuerung durch den Gyroscope Sensor aktiviert. Dieses erlaubt dem Benutzer durch die Neigungswinkeländerung des FernDuinos das Segway zu steuern. Der maximale Neigungswinkel für eine Achse liegt bei 20° - dieses entspricht dem Vollausschlag des linken analogen Joysticks in eine bestimmte Achse.
Reglereinstellungen
Der Druckknopf des rechten analogen Joysticks ist für die Veränderung der Reglereinstellungen des Segways. Durch Betätigen des Knopfes gelangt der Benutzer in das Menü. Dem Benutzer wird durch ein ">" angezeigt, welchen Wert er mit dem rechten Joystick gerade einstellen kann. Ein leichtes Wippen nach rechts oder links erhöht oder senkt den Regelanteil jeweils um eins (I = 0.01) und ein Vollausschlag in eine bestimmte Richtung um 10 (I = 0.1). Durch erneutes Drücken des Knopfes wird der nächste Parameter zum Verstellen freigegebenen. Währenddessen läuft das Segway mit den alten Reglerwerten weiter, bis der Benutzer durch ein letztes Drücken des rechten Joysticks wieder in das Steuerungsmenü gelangt. Erst dann werden die neuen Reglerwerter übertragen.
Projektplan
Anhand des nachfolgenden Projektplans sollen die einzelnen Schritte des Projektes "Segway 2.0 und FernDuino" von der Planung bis zur Präsentation erläutert und in Kurzform beschrieben werden.
Nachdem die Genehmigung für die Fortführung des Projektes erteilt worden ist, begann die eigentliche Planung des Projektes. In diesem Schritt wurden vor allem die neuen Features des Segways 2.0 besprochen und wie diese in Kombination mit dem FernDuino umgesetzt werden konnten. Zudem gab es keinen Prototypen des FernDuinos, sodass die technischen Einzelheiten vor der Materialbestellung geklärt werden mussten. Nachdem die benötigte Materialliste angefertigt wurde, konnte die theoretische Realisierung des Segways 2.0 und des FernDuinos gestartet werden. Dabei standen der technische und mechanische Aufbau durch Skizzen und Planungen im Vordergrund. Parallel dazu wurde die Software und der mechanische Aufbau des Segways 1.0 verbessert. Da mit einer langen Wartezeit bei der Bestellung der benötigten Teile gerechnet wurde, wurden die einzelnen Teile des FernDuinos vorab in einer Eigenbestellung geordert und die technische Realisierbarkeit und Programmierung auf einem Steckbrett getestet. Als die Bauteile Mitte Oktober geliefert wurden, konnte die Bauphase der beiden Systeme beginnen. Hierzu wurden zeitgleich die Erfolge in der Dokumentation festgehalten. Beim Testen des Segways 2.0 fielen die falsch gelieferten Motoren auf, da diese ein anderes Übersetzungsverhältnis und somit eine andere Dynamik als die im Segway 1.0 hatten. Aufgrund dessen sind die Motortreiber beschädigt worden, sodass eine Neubestellung der Treiber und Motoren beantragt wurde. Nach der Lieferung musste die Bodenplatte des Chassis überarbeitet werden. In der letzten Woche vor der Präsentation konnten die beiden Systeme zum ersten Mal gemeinsam in Betrieb genommen werden. Hier wurden vor allem Tests und Optimierungen beider Systeme durchgeführt. Die Präsentation am 22.01.2018 hat das komplette Projekt vollendet.
Fazit
Die beiden Projekte Segway 2.0 und FernDuino haben von der Entwicklung bis zur Fertigstellung und den ersten Tests sehr viel Spaß gemacht und waren sehr lehrreich. Leider ist der Umfang des Projektes komplett aus den Rudern gelaufen, da einerseits das alte Segway 1.0 mechanisch und softwaretechnisch überarbeitet und die neuen beiden Systeme komplett neu konstruiert werden mussten. Aus diesem Grund wurde das gemeinsame Projekt zu zwei einzelnen Projekten (Segway 1.0 und Segway 2.0 / FernDuino) ausgegliedert. Erst in der späteren Projektphase konnten die beiden einzelnen Projekte gemeinsam erfolgreich getestet und optimiert werden.
Mögliche Verbesserungen
- Mechanischen Aufbau des FernDuinos ändern
Problem: Bei der Nutzung des FernDuinos sind die zu eng platzierten Joysticks negativ aufgefallen. Dadurch stößt der Daumen immer wieder gegen das LCD Modul.
Lösung: Joystickpositionen verändern oder das LCD Modul erhöhen. Besser wäre ein kompletter Aufbau des Gehäuses aus einem 3D-Drucker. Dieses impliziert ebenfalls eine Neuentwicklung der Platine. Der derzeitige Aufbau ist modular mit einer Lochrasterplatine aufgebaut. Eleganter ist die Lösung einer eigenentwickelten Platine mit eingebetteten Modulen. Durch diesen Aufbau entfallen ebenfalls die Litzen der einzelnen Module, die sehr fehleranfällig sein können.
- Bidirektionale Verbindung
Problem: Die derzeitige Funkverbindung vom FernDuino zum Segway ist unidirektional. Eine Funkunterbrechnung kann der FernDuino nicht erkennen und sendet immer weiter bis das Segway wieder gefunden wurde. Dadurch können Datenlecks entstehen.
Lösung: Die Bibliothek des nRF24L01+ Funkmoduls ist ebenfalls für eine bidirektionale Verbindung vorgesehen. Dieses kann im Nachhinein in der Software per ACK noch implementiert werden. Zudem können durch diese Änderungen einige Neuerungen im Menü vorgenommen werden, wie z.B. Anzeige der Ultraschallentfernung vom Segway zum nächsten Hindernis, Funkunterbrechungen, Akkustand des Segways und des FernDuinos u.s.w.
- Reglerparametierung
Problem: Die derzeitigen Reglerparameter des Segways sind nach der "Try and Error" Methode ausgelegt worden und damit nicht optimal ausgelegt.
Lösung: Komplette Simulation des Segways.
YouTube Video
→ zurück zum Hauptartikel: Fachpraktikum Elektrotechnik (WS 15/16)