Die Motoren können über den PWM-Controller gesteuert werden. Die drive(PWM0, PWM1, PWM2, PWM3) Funktion nimmt dabei vier Werte entgegen, die jeweils zwischen 0 und 255 liegen. Zusätzlich ist hier ein Funktion eingefügt um die Motoren einzeln zu verzögern und somit einem produktionsbedingtem Drehzahlunterschied der Motoren entgegenzuwirken. Hierüber wird die Motorgeschwindigkeit definiert. PWM0 und PWM1 definieren dabei den linken Motor und steuern die Vorwärts- und Rückwärtsbewegung. PWM2 und PWM3 definieren dementsprechend den rechten Motor.
Die zusätzliche Funktion stop() setzt alle Motorbewegungen auf 0 und führt damit ein Bremsmanöver durch.
# Import des micro:bit Moduls
from microbit import *
# Initialisierung der I2C Schnittstelle
i2c.init(freq=400000, sda=pin20, scl=pin19)
# Initialisierung des PWM Controllers
i2c.write(0x70, b'\x00\x01')
i2c.write(0x70, b'\xE8\xAA')
# Durch die Verzögerung eines Motors kann eine durch produktionsbedingte
# Toleranzen unterschiedliche Drehzahl der Motoren ausgeglichen werden.
biasR = 0 # Verzögerung des rechten Motors in Prozent
biasL = 0 # Verzögerung des linken Motors in Prozent
# Die scale Funktion wird genutzt um die bias-Variablen für
# die Berechnung der Motorgeschwindigkeit umzuskalieren.
def scale(num, in_min, in_max, out_min, out_max):
return (num - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
# Motoren mithilfe des PWM Controllers steuern
# PWM0 und PWM1 für den linken und PWM2 und PWM3 für den rechten Motor
def drive(PWM0, PWM1, PWM2, PWM3):
# Skalierung des Verzögerungswerts in den Wert in Prozent mit dem
# der Motor drehen soll. Bsp: bias = 5 wird in 95 umgewandelt
# und anschliessend durch 100 geteilt. 255 * (95/100).
PWM0 = int(PWM0 * (scale(biasR, 0, 100, 100, 0) / 100))
# Wiederholen des Vorgangs für alle 4 Kanäle
PWM1 = int(PWM1 * (scale(biasR, 0, 100, 100, 0) / 100))
PWM2 = int(PWM2 * (scale(biasL, 0, 100, 100, 0) / 100))
PWM3 = int(PWM3 * (scale(biasL, 0, 100, 100, 0) / 100))
# Wert für PWM Kanal (0-255) an PWM Controller übertragen.
# 0x70 ist die I2C Adresse des Controllers. b'\x02 ist
# das Byte für den PWM Kanal 1. Zu dem Byte für den
# Kanal wird das Byte mit dem PWM Wert zu addiert.
i2c.write(0x70, b'\x02' + bytes([PWM0]))
# Wiederholen des Vorgangs für alle 4 Kanäle
i2c.write(0x70, b'\x03' + bytes([PWM1]))
i2c.write(0x70, b'\x04' + bytes([PWM2]))
i2c.write(0x70, b'\x05' + bytes([PWM3]))
# alle Motoren stoppen
def stop():
drive(0, 0, 0, 0)
# Variablen für die Demo-while-Schleife
speed = 35
direction = "f"
# Demo-Schleife
while True:
# vorwärts fahren
if direction == "f":
speed += 1
drive(speed, 0, speed, 0)
sleep(30)
if speed > 254:
stop()
print("rückwärts")
sleep(1000)
speed = 35
direction = "b"
# rückwärts fahren
elif direction == "b":
speed += 1
drive(0, speed, 0, speed)
sleep(30)
if speed > 254:
stop()
print("links")
sleep(1000)
speed = 35
direction = "l"
# links fahren
elif direction == "l":
speed += 1
drive(0, speed, speed, 0)
sleep(30)
if speed > 254:
stop()
print("rechts")
sleep(1000)
speed = 35
direction = "r"
# rechts fahren
elif direction == "r":
speed += 1
drive(speed, 0, 0, speed)
sleep(30)
if speed > 254:
stop()
print("vorwärts")
sleep(1000)
speed = 35
direction = "f"
# sonst stopp
else:
stop()
print("gestoppt")