Motorsteuerung mit Gyroskop

Python auf Einplatinencomputer wie Raspberry Pi, Banana Pi / Python für Micro-Controller
Antworten
VolkerPi
User
Beiträge: 5
Registriert: Sonntag 4. Juni 2017, 19:24

Hallo,

Ich möchte mit meinem Script bewirken, dass sich mein Bot um die angegebene Grad-Zahl dreht.
Ich verwende ein Pi 3 mit dem Modul HMC883l sowie einer Motorsteuerung.
Damit ich ein Verständnis dafür bekomme, sollen noch einige andere Sensoren (DHT22, GPS, Ultraschall...) hinzukommen und miteinander zusammen kommunizieren.

Mein Script sieht bisher so aus:

Code: Alles auswählen

#!/usr/bin/python

import time, smbus, math
import L298NHBridge as HBridge

speed = 0.7
bus = smbus.SMBus(1)
address = 0x1e              #  check: i2cdetect -y 1

turn =input("Grad Zahl fuer die Drehung des Roboter-Autos: ")   

# Pruefen ob die eingegebene Grad Zahl Sinn macht. 
if turn <= 0 or turn >= 360:
   print "--------------------------------------------"
   print "Bitte eine Gradzahl zwischen >0 und <360 eingeben."
   print "--------------------------------------------"

# +++++++++++++++++++++++++++++++++
# +     Gyroskop auslesen         +
# +++++++++++++++++++++++++++++++++

def read_byte(adr):
   return bus.read_byte_data(address, adr)

def read_word(adr):
   high = bus.read_byte_data(address, adr)
   low = bus.read_byte_data(address, adr+1)
   val = (high << 8) + low
   return val

def read_word_2c(adr):
   val = read_word(adr)
   if (val >= 0x8000):
      return -((65535 - val) + 1)
   else:
      return val

def write_byte(adr, value):
   bus.write_byte_data(address, adr, value)

   
    
# +++++++++++++++++++++++++++++++++
# +       Motorsteuerung          +
# +++++++++++++++++++++++++++++++++

# So lange stopp = 0 arbeitet dieser Teil in der
# Endlosschleife
stopp = 0

# Startwinkel speichern
write_byte(0, 0b01110000) # Set to 8 samples @ 15Hz
write_byte(1, 0b00100000) # 1.3 gain LSb / Gauss 1090 (default)
write_byte(2, 0b00000000) # Continuous sampling

scale = 0.92

time.sleep(0.1)
x_out = read_word_2c(3) * scale
y_out = read_word_2c(7) * scale
z_out = read_word_2c(5) * scale

z  = math.atan2(y_out, x_out) 
if (z < 0):
   z += 2 * math.pi

angle_start = int(math.degrees(z))

while stopp == 0:
   print "Speed: ",speed

   angle_run = int(math.degrees(z))

   move = angle_start-turn
   
   if turn <= 180:
      if turn > 0:
         # Ist speed postiv drehen sich die Motoren vorwaerts.
         # Ist speed negativ drehen sich die Motoren rueckwaerts.
         #HBridge.setMotorLeft(speed)
         #HBridge.setMotorRight(-speed)
         print move, " Grad <= 180 Aktuell: ",angle_run 
      elif turn < 0:
         #HBridge.setMotorLeft(-speed)
         #HBridge.setMotorRight(speed)
         print move, " Grad < 0  Aktuell: ",angle_run
   elif turn > 180:
      if turn > 0:
         #HBridge.setMotorLeft(-speed)
         #HBridge.setMotorRight(speed)
         print move, " Grad > 0  Aktuell: ",angle_run
      elif turn < 0:
         #HBridge.setMotorLeft(speed)
         #HBridge.setMotorRight(-speed)
         print move, " Grad < 0  Aktuell: ",angle_run

   # Wenn die Abweichung zum Ziel lediglich 
   # +-15 Grad betraegt verlangsamen die Motoren.
   if angle_run - 15 < turn < angle_run + 15:
      speed = 0.5         
      print "langsamer, Differenz +-15 Grad"
          
   # Wenn die Abweichung zum Ziel lediglich +-2 Grad betraegt
   # stoppen die Motoren und das Programm wird beendet.
   if angle_run - 2 < turn < angle_run + 2:
      stopp = 1.0
      print "Stopp, das Ziel ist erreicht!"
          
   time.sleep(0.04)
Zur besseren Verständnis habe ich die Motorsteuerung erst mal ausgeblendet.

Sobald ich das Starte, möchte ich einen Drehwinkel einstellen. Die Motoren sollen so lange laufen, bis dieser Winkel erreicht wurde.
Es soll auch noch unterscheiden können, ob er schneller nach links dreht, um das Ziel zu erreichen, oder es besser ist, nach rechts zu drehen.

Was passiert derzeit:

- ich kann den Winkel eingeben
- der Winkel (angle_start) wird auch ausgelesen und angezeigt, jedoch nicht verändert (angle_run).
- die Motoren drehen endlos im Kreis

Die Ausgabe sieht so aus:
  • Grad Zahl fuer die Drehung des Roboter-Autos: 100
    Speed: 0.7
    74 Grad <= 180 Aktuell: 174
    Speed: 0.7
    74 Grad <= 180 Aktuell: 174
    Speed: 0.7
    74 Grad <= 180 Aktuell: 174
    Speed: 0.7
    74 Grad <= 180 Aktuell: 174
    Speed: 0.7
    74 Grad <= 180 Aktuell: 174
    Speed: 0.7
    74 Grad <= 180 Aktuell: 174
    Speed: 0.7
    74 Grad <= 180 Aktuell: 174
    Speed: 0.7
    74 Grad <= 180 Aktuell: 174
    Speed: 0.7
    74 Grad <= 180 Aktuell: 174
Ich bin Neuling und habe mir den Code irgendwie zusammen gebastelt.

Was mir nicht so einleuchtet ist, dass sich der Winkel nicht ändert. Nun ich vermute mal, das liegt im Zusammenspiel mit dem Code hier:

Code: Alles auswählen

# Startwinkel speichern
write_byte(0, 0b01110000) # Set to 8 samples @ 15Hz
write_byte(1, 0b00100000) # 1.3 gain LSb / Gauss 1090 (default)
write_byte(2, 0b00000000) # Continuous sampling

scale = 0.92

time.sleep(0.1)
x_out = read_word_2c(3) * scale
y_out = read_word_2c(7) * scale
z_out = read_word_2c(5) * scale

z  = math.atan2(y_out, x_out) 
if (z < 0):
   z += 2 * math.pi

angle_start = int(math.degrees(z))
...welcher außerhalb der While-Anweisung steht.

Jetzt die Frage:
Wie optimiere ich das? Es ist so, dass "angle_run" sich verändern muss, "turn" ist die Grad-Zahl der Eingabe und "move" ist ja das Ziel.

...ich bin leider nicht so fit in der Sache. Ich wäre dankbar, mich in dieser Sache zu unterstützen.

ich danke euch.
v
Zuletzt geändert von Anonymous am Sonntag 4. Juni 2017, 21:04, insgesamt 1-mal geändert.
Grund: Quelltext in Python-Codebox-Tags gesetzt.
Siegfried
User
Beiträge: 24
Registriert: Sonntag 30. April 2017, 14:04

@ VolkerPi: Dein Programm läuft in einer Endlosschleife, weildie Abbruchbedingung niemals erfüllt ist; sie ist nie erfüllt, weil die Werte der Variablen sich nicht ändern. turn wird nur einmal eingelesen und danach nicht mehr verändert, angle_run wird in der Schleife immer wieder auf z gesetzt, z wird aber nur einmal (außerhalb der Schleife) berechnet und verändert sich daher nicht mehr. Wie Du schon richtig vermutet hast, musst Du also innerhalb der Schleife angle_run stets aufs Neue ermitteln.

Wenn Du den Fehler behoben hast,solltest Du einige "Aufräumaktionen" durchführen:
Du warnst, wenn für turn Werte kleiner 0 oder größer 360 eingegeben werden, brichst aber das Programm nicht ab. In der Schleife prüfst Du sogar auf negativen turn. Die Abbruchbedingung wird bei negativem turn aber nie erreicht, d.h. schon wieder Endlosschleife. Analog wenn turn-Eingabe über 360 ist
Du berrechnest z_out, benutzt es aber nie.
Deine Variable stopp hat nur die zwei Werte 0 und 1.0, d.h. zwei verschiedene Datentypen. Bei zwei möglichen Zuständen wäre es sinnvoller, ´True´ und ´False´ zu wählen und dann die Schleife in der Form while not stopp: auszuführen oder gleich ganz auf stopp zu verzichten in der Form while True und in der Abbruchbedingung statt stopp = 1.0 nur break zu schreiben.
VolkerPi
User
Beiträge: 5
Registriert: Sonntag 4. Juni 2017, 19:24

Hallo Siegfried,

danke für die Rückantwort. Ich habe die Schleife geändert und mit "true" gearbeitet.
...das war der einfachste teil :-)

...aber, wie ich dich verstanden habe, muss die Berechnung des Winkels 2x ausgeführt werden. Ich will mein Code effektiv gestalten und diesen Teil nicht 2x verwenden. Gibt es sowas wie Sprungmarken?

Code: Alles auswählen

def get_angle(z):
# Startwinkel speichern
   write_byte(0, 0b01110000) # Set to 8 samples @ 15Hz
   write_byte(1, 0b00100000) # 1.3 gain LSb / Gauss 1090 (default)
   write_byte(2, 0b00000000) # Continuous sampling

   scale = 0.92

   time.sleep(0.1)
   x_out = read_word_2c(3) * scale
   y_out = read_word_2c(7) * scale

   z  = math.atan2(y_out, x_out) 
   if (z < 0):
      z += 2 * math.pi
...kann man sich die Arbeit dadurch ersparen und aus der Schleife aufrufen? ...oder gibt es eine andere Lösung?

...als nächstes kümmere ich mich um die Berechnung...

Danke vorab
v
VolkerPi
User
Beiträge: 5
Registriert: Sonntag 4. Juni 2017, 19:24

Hallo,

ich habe mal wieder etwas am Script gebastelt. Nein, fertig ist es noch nicht. Ich versuche die Tipps von Sigfried umzusetzen.

Ich weiss nicht, warum hier ein Syntax-Fehler heraus kommt...

Code: Alles auswählen

.
.
.
def get_angle():
    write_byte(0, 0b01110000) # Set to 8 samples @ 15Hz
    write_byte(1, 0b00100000) # 1.3 gain LSb / Gauss 1090 (default)
    write_byte(2, 0b00000000) # Continuous sampling

    scale = 0.92

    time.sleep(0.1)
    x_out = read_word_2c(3) * scale
    y_out = read_word_2c(7) * scale
    # z_out = read_word_2c(5) * scale
    
    bearing  = math.atan2(y_out, x_out)
    
    if (bearing < 0):
        bearing += 2 * math.pi
        return (bearing)
   
stopp = 0

z = get_angle()
angle_start = int(math.degrees(z)

while stopp == True:
.
.
.

Wenn ich den Teil bis zur While-Schleife in ein separates Script speichere und starte, bekomme ich ein Winkel angezeigt.
Warum kommt bei While jetzt ein Fehler?

Grüße
v
BlackJack

@VolkerPi: Ich bekomme den Syntaxfehler ja schon in der ersten Zeile bei dem . der so da nicht stehen darf.

Ansonsten: Auch ohne das ``while`` würdest Du einen Syntaxfehler bekommen. Der Compiler erwartet an der Stelle etwas ganz bestimmtes, damit der Code der davor steht gültig ist.
Siegfried
User
Beiträge: 24
Registriert: Sonntag 30. April 2017, 14:04

@VolkerPi : Ich glaube wir sollten noch einmal die Grundlagen der Programmierung (GrdlPrg) durch gehen. Du solltest zuerst versuchen, das Problem in normaler Sprache zu strukturieren. Immer die Frage stellen, "was will ich und wie kann ich es erreichen". Was willst Du? --> eine Motorsteuerung. Wie kannst Du es erreichen? --> einen Soll-Winkel einlesen und dann die Motore so lange aktivieren, bis der gewünschte Soll-Winkel erreicht ist. Damit ist schon viel erreicht; Du hast hast zum einen zwei kleinere Teilprobleme und zum anderen einen "Schlüsselausdruck" der deutschen Sprache benutzt ("Solange...bis"), der in Python eine Entsprechung durch `while` hat.
Nun kannst Du mit den Teilproblemen wie oben fortfahren, wobei das "was will ich" immer das "wie kann ich es erreichen" der vorigen Ebene ist.
"Was": einen Soll-Winkel einlesen --> raw_input Teilproblem gelöst.
"Was": solange aktivieren bis ... --> Schleife, d.h. den Ist-Winkel ermitteln und mit dem Soll-Winkel auf Gleichheit prüfen (Abbruchbedingung). Wenn Ja dann Schleife verlassen, sonst Motor starten und wieder zum Schleifenanfang.
"Was": Ist-Winkel ermitteln --> Aufruf der vorhandenen Funktion
"Was"": Prüfung auf Gleichheit --> erneut ein "Schlüsselausdruck" diesmal für die bedingte Anweisung bzw `if`

Damit steht die Grundstruktur:
[codebox=pys60 file=Unbenannt.txt]
vorhandene "Standardgeschichten" (import ..., speed, bus, adress, alle 4 Funktionen(,die mit `def` anfangen)

Soll-Winkel einlesen
while True: ## Schleifenanfang
Ist-Winkel durch Funktionsaufruf in ermitteln
if Ist-Winkel == Soll-Winkel: ## Abbruchbedingung
Abbruch
Starte Motor

[/code]

Lass am Anfang alles weg, was nicht zwingend notwendig ist (Soll-Wert Prüfung, Motorgeschwindigkeit verändern usw). Diese Sachen können später nachgetragen werden. Besser ist es, in der Erstellungs- und Prüfphase Test- und Zwischenwerte auszugeben. So könntest Du nach dem Einlesen des Soll-Winkels ihn gleich einmal mit der `print`-Funktion ausgeben lassen, um zu gucken ob Dein Programm mit dem Wert rechnet, den Du Dir vorstellt. Das Gleiche gilt für den Ist-Winkel. Du kannst dann erkennen, ob der Ist-Wert sich bei laufendem Motor verändert. Wenn Ja, dann ist alles in Ordnung; wenn nein überprüfe noch einmal das Ermitteln des Ist-Winkels. Wenn der Winkel sich verändert aber das Programm nicht stoppt, liegt es vielleicht an unserer Abbruchbedingung . Da wir auf exacte Gleichheit prüfen, kann (und wird) es sein, daß der Ist-Winkel zwischen zwei Auslesungen schon größer als der Soll-Winkel geworden ist, d.h. wir sollten statt `==` lieber `>=` schreiben.

Wenn es so weit funktioniert, kannst Du die Feinheiten wie Motorgeschwindigkeit usw implementieren. Viel Erfolg!
VolkerPi
User
Beiträge: 5
Registriert: Sonntag 4. Juni 2017, 19:24

Hallo Sigfried,

ich bin gerade am umbauen. Ein paar Sachen will ich gerade umsetzen und ab und an teste ich die Sache.

ich hänge aber an einer Sache:

Code: Alles auswählen

def get_angle():
    write_byte(0, 0b01110000) # Set to 8 samples @ 15Hz
    write_byte(1, 0b00100000) # 1.3 gain LSb / Gauss 1090 (default)
    write_byte(2, 0b00000000) # Continuous sampling

    scale = 0.92

    time.sleep(0.1)
    x_out = read_word_2c(3) * scale
    y_out = read_word_2c(7) * scale
    
    bearing  = math.atan2(y_out, x_out)
    
    if (bearing < 0):
        bearing += 2 * math.pi
        bearing = float(bearing)
        return (bearing)
   
while True:
   # ich benötige die aktuelle Position, wo bin ich?
   angle = get_angle()
   angle_start = math.degrees(angle)
   
   move = angle_start-turn #das ist das Zeil
warum will es ein Float? ich muss zu geben, dass ich vor 2 Tagen mein Pi auf den neuesten Stand gebracht habe. seit dem ist der Wurm drin.
naja, ich bin nicht sicher, ob das Einfluss auf Python hat.

Was vorgestern noch beim Script ohne Motorsteuerung funktioniert, klappt heute nicht mehr. :K

Liegt es daran, dass ich alles in ein def get_angle() gepackt habe? ...es hat doch aber funktioniert?!?!?!?

ich geh kaputt...

grüße
v
BlackJack

@VolkerPi: Warum will was ein Float? Zeile 16 ist überflüssig, denn `bearing` ist da bereits eine Gleitkommazahl weil math.atan2() eine Gleitkommazahl liefert.

Weder um die Bedingung beim ``if`` noch um den Rückgabewert gehören klammern.

Kann es sein, dass das ``return`` falsch eingerückt ist? Falls nicht, sollte man explizit ``return None`` schreiben, damit das klarer ist.
VolkerPi
User
Beiträge: 5
Registriert: Sonntag 4. Juni 2017, 19:24

Hi,

ich habe nun alles soweit umgesetzt. ich habe auch keine Fehler mehr. Ich muss noch hin bekommen, dass die Drehung funktioniert. Jetzt werden die Winkel wieder angezeigt und diese ändern sich auch.

Mein Ziel ist ja, das Fahrzeug um die eingegebene Grad-Zahl zu drehen, also ziel = eingegebene Gradzahl - istPosition. Ich kümmere mich jetzt um die Steuerung der 2 Motoren.

Ich habe aber noch eine Frage bezüglich dem Input. Ich habe gelesen, dass raw_input für Strings ist, ich benötige aber eine Ganzzahl.
Liege ich da richtig oder gibt es noch was anderes zum Unterschied beider Anweisungen?

Grüße
v
BlackJack

@VolkerPi: Das wichtigste was man zu `input()` in Python 2 wissen sollte, ist das man es nicht verwenden sollte. Das ist zum eingeben beliebiger Python-Ausdrücke die dann ausgewertet werden. Im einfachsten Fall ist es nahezu unmöglich sämtliche Fehleingaben sinnvoll zu behandeln, und im schlechtesten Fall gibt jemand absichtlich etwas ein das in das Programm eingreift, oder Dateien löscht, oder…

`raw_input()` versucht nichts zu interpretieren, sondern liefert die Benutzereingabe als Zeichenkette. Mit der kann man dann gezielt das machen was man will. Zum Beispiel sie in eine Gleitkommazahl umwandeln, und dabei dann auch auf die einzige Ausnahme reagieren die diese Aktion auslösen kann wenn der Benutzer etwas eingibt, das keine Zeichenkettendarstellung einer Gleitkommazahl ist: `ValueError`.
Antworten