Drehung mit IMU

Soll ein Roboter eine exakte Drehung um einen bestimmten Winkel durchführen, so kann diese exakter ausgeführt werden, wenn man den IMU-Sensor verwendet und nicht nur die Motoren mit unterschiedlichen Geschwindigkeiten für eine bestimmte Zeit drehen lässt.

Da sich der YAW-Wert des IMU-Sensors nicht zurücksetzen lässt, sondern als Wert immer den absoluten Drehwinkel seit dem Einschalten des Controllers liefert, muss zu Beginn einer Drehung der aktuelle Winkel bestimmt und in der Folge als Referenz für die gewünschte Drehung verwendet werden.

Im folgenden Beispiel wird vor Beginn der Drehung des Roboters um die Hochachse der aktuelle YAW-Wert des IMU-Sensors ermittelt und in der Variable StartWert abgelegt.

Innerhalb der while-Schleife wird zunächst der aktuelle YAW-Wert des IMU-Sensors eingelesen. Wenn die Drehung genau 90 Grad betragen soll, wird in einer if-Abfrage während der Drehung überprüft, ob der aktuelle Wert gleich dem Startwert plus 90 Grad ist.

Da es sein könnte, dass die YAW-Werte des IMU-Sensors nicht kontinuierlich in 1-Grad-Schritten zurückgeliefert werden, oder sich der Roboter zwischen zwei Abfragen mehr als 1 Grad gedreht hat, sollte man als Vergleichsoperator >= und nicht nur == verwenden. Somit würde die Bedingung der if-Abfrage auch erfüllt sein wenn der IMU-Sensor z. B. zunächst den Wert 89 Grad und beim nächsten Auslesen den Wert 91 liefert.

Beispiel - Exakte Drehung mit dem Gyro-Sensor ausgehend von einer beliebigen Startrichtung

import cyberpi, mbot2
 
int_StartWert = cyberpi.get_yaw()
 
mbot2.EM_set_speed(-10, 1)
mbot2.EM_set_speed(-10, 2)
 
while True:
    int_AktuellerWert = cyberpi.get_yaw()
 
    str_ausgabe = "S: " + str(int_StartWert) + " A: " + str(int_AktuellerWert)
    cyberpi.display.show_label(str_ausgabe,16,0,0)
 
    int_SollWert = int_StartWert + 90
    if int_AktuellerWert>=int_SollWert:
        mbot2.EM_stop()

Erklärungen zu diesem Beispiel

Zeile 3: int_StartWert = cyberpi.get_yaw()

An dieser Stelle wird der Integer-Variable int_StartWert der aktuelle YAW-Wert des IMUSensors beim Start des Skripts zugewiesen.

Zeile 5, 6: mbot2.EM_set_speed(-10, 1)

Mit Start des Skripts beginnen sich die Motoren entgegengesetzt zu drehen.

Zeile 9: int_AktuellerWert = cyberpi.get_yaw()

Einlesen des aktuellen YAW-Werts des IMU-Sensors und Ablegen in der Variable int_AktuellerWert.

Zeile 11-12: str_ausgabe = "S: " + str(int_StartWert) + " A: " + str(int_AktuellerWert)

Zusammenstellen eines Strings zur Anzeige des Startwerts und der aktuellen YAW-Werts des IMU Sensors.

Zeile 14: int_SollWert = int_StartWert + 90

Berechnung des gewünschten Ziels für den YAW-Wert und ablegen in der Variable int_SollWert.

Zeile 15: if int_AktuellerWert>=int_SollWert:

Überprüfung, ob der der Sollwert erreicht ist. Wenn der aktuelle YAW-Wert größer oder gleich dem gewünschten Zielwert ist, dann soll der Roboter anhalten.

ACHTUNG - wichtige Anmerkungen zu diesem Beispiel

1. Keine kontinuierlichen YAW-Werte

Bei der Bedingung zwischen dem aktuellen Wert und dem Sollwert in der if-Abgrabe Zeile 15 ist UNBEDINGT zu beachten, wie sich die YAW-Werte des IMU-Sensors in Rahmen einer 360° Drehung ändern. Ausgehend von 0°, wenn der CyberPi-Controller Strom bekommt, werden die Drehungen mit Werten zwischen 0° bis 180° bzw. 0° bis -180° zurückgegeben. Somit sind Sprünge von -180° auf +180° und umgekehrt bei der Berechnung des Sollwerts, wie auch bei den Bedingung der if-Abfrage zu berücksichtigen, wenn das Skript aus jeder beliebigen Startrichtung funktionieren soll!

2. Zeitintensive Ausgabe am Display

Bei diese Skript bemerkt man, dass Ausgaben am Display sehr viel Zeit beanspruchen. Man erkennt dies einerseits daran, dass sich die Anzeige der Zahlen des Startwerts und des aktuellen Werts nicht genau um 90° unterscheiden, sondern bis zu 10° darüber liegen. Andererseits kann man auch mit freiem Auge erkennen, dass der Roboter deutlich mehr als 90° dreht.

Dies tritt leider auf, da das Schreiben von Text auf das Display aufgrund der internen Hardwarelösung, wie auch der Software-technischen Umsetzung sehr viel Zeit in Anspruch nimmt und die weitere Ausführung des Skripts beim Aufruf der Funktion cyberpi.display.show_label() angehalten wird.

Daraus ergibt sich, dass bei zeitkritischen Abläufen und Sensorauswertungen, die sehr oft hintereinander ausgeführt werden müssen auf Ausgaben am Display verzichtet werden muss.

Bei diesem Beispiel kann dies sehr leicht beobachtet werden, wenn man die beiden Zeilen zu Ausgabe einfach mit einer # davor auskommentiert, oder erst beim Stoppen in der if-Abfrage ausführen lässt.

Modifikation 1 - Auskommentieren der Ausgabe am Display

import cyberpi, mbot2
 
int_StartWert = cyberpi.get_yaw()
 
mbot2.EM_set_speed(-10, 1)
mbot2.EM_set_speed(-10, 2)
 
while True:
    int_AktuellerWert = cyberpi.get_yaw()
 
    #str_ausgabe = "S: " + str(int_StartWert) + " A: " + str(int_AktuellerWert)
    #cyberpi.display.show_label(str_ausgabe,16,0,0)
 
    int_SollWert = int_StartWert + 90
    if int_AktuellerWert>=int_SollWert:
        mbot2.EM_stop()

Modifikation 2 - Anzeige erst nach erfolgter, exakter Sensorauswertung

import cyberpi, mbot2
 
int_StartWert = cyberpi.get_yaw()
 
mbot2.EM_set_speed(-10, 1)
mbot2.EM_set_speed(-10, 2)
 
while True:
    int_AktuellerWert = cyberpi.get_yaw()
 
    int_SollWert = int_StartWert + 90
    if int_AktuellerWert>=int_SollWert:
        mbot2.EM_stop()
        str_ausgabe = "S: " + str(int_StartWert) + " A: " + str(int_AktuellerWert)
        cyberpi.display.show_label(str_ausgabe,16,0,0)

Exakte Drehung eines Roboters mit dem IMU-Sensor