Das deutsche Python-Forum

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)

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))

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

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:
.
.
.

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