Raspberry Pi Pico W jetzt mit Bluetooth - Teil 2 - Robot Car und Contr

0 Kommentare

Im ersten Teil hatten wir vorgestellt, wie der Raspberry Pi Pico W mit den Temperatursensoren BME280 und DHT20 lokal die Temperatur und Luftfeuchtigkeit auf einem 0,96“ bzw. 1,3“ OLED-Display anzeigt und zugleich die Daten über Bluetooth BLE an ein Smartphone oder Tablet sendet. Diesmal möchte ich mein Lieblingsthema Robot Cars behandeln, zunächst mit einem Pico W auf dem Chassis und einem für den Controller. Grundlage der Programme sind die Beispiele
ble_simple_peripheral.py,

ble_simple_central.py,

und als Modul auf dem Pico W die Datei ble_advertising.py,

die man auf Github unter https://github.com/micropython/micropython/tree/master/examples/bluetooth
findet. Am besten gleich alle „Examples“ als zip-Datei herunterladen.

Für die Verwendung von Bluetooth mit dem Raspberry Pi Pico W benötigen Sie die neueste Firmware, die seit Juni 2023 außer WiFi auch Bluetooth unterstützt. Beim Schreiben des Blog-Beitrags war es diese Datei: micropython-firmware-pico-w-130623.uf2. Dann hält man den BOOTSEL Button gedrückt und verbindet den Pico W über USB mit dem PC. Im Explorer erscheint der Pico wie ein USB-Stick, so dass man die Datei *.uf2 mit „drag and drop“ auf den Pico verschieben kann. Wenige Sekunden später verschwindet das USB-Laufwerk und der Pico W kann mit Thonny programmiert werden.

Als hilfreiche Quellen möchte ich noch einmal die folgenden Internetseiten empfehlen:

https://www.raspberrypi.com/documentation/microcontrollers/raspberry-pi-pico.html

https://datasheets.raspberrypi.com/picow/connecting-to-the-internet-with-pico-w.pdf

https://docs.micropython.org/en/latest/library/bluetooth.html

Verwendete Hardware

2	Raspberry Pi Pico W mit aktueller Firmware
1	Smart Robot Car Kit
alt	beliebiger Bausatz mit Chassis und Rädern/Motoren und L298N Motor Controller Board mit DC-DC-Converter 5V (Teil des o.g. Kits)
2	Breadboard, Jumperkabel, Batteriekasten mit 4 AA-Batterien
	PC mit Thonny, Android Smartphone/Tablet

Schaltpläne

Programmcode

Das Beispielprogramm ble_simple_peripheral.py bildet die Grundlage für das Programm auf dem Robot Car. Wichtig ist, dass das MicroPython-Beispielprogramm ble_advertising.py als Modul auf diesen Pico W kopiert wird. Ansonsten erhält man bei der Zeile „from ble_advertising import advertising_payload“ eine Fehlermeldung. So komisch es klingen mag, das Peripheriegerät bietet den Bluetooth LE-Dienst an, der Controller (s.u. Programmbeispiel auf der Basis von ble_simple_central.py) sucht den Anbieter anhand der UUID und verbindet sich dann.

Eine kurze Erläuterung zu UUID (universally unique identifier). So einmalig, wie der Name klingt, müssen die nicht sein. Wenn Ihr Gerät weit und breit das Einzige ist, können die UUIDs wiederverwendet werden. Ansonsten lässt man sich auf der Internet-Seite https://www.uuidgenerator.net/ neue und einmalige UUIDs generieren.

Die gegenüber den Beispielprogrammen vorgenommenen Änderungen befinden sich zu Beginn beim Importieren und Instanziieren der Pins für die Motorsteuerung und der selbst-definierten Funktion motor(cy,cx) mit zwei Parametern. Im Hauptteil am Ende wird der empfangene Code in seine Bestandteile zerlegt - die ersten zwei Ziffern für vor bzw. zurück, die zweiten beiden Ziffern für links/rechts und die letzte Ziffer für den Joystick-Button. Die Wertebereiche der beiden Joystick-Potis werden „gemapped“ auf den Bereich 0 bis 31; die Mittelposition liegt bei 16. Diesen Wert subtrahiere ich von dem jeweiligen Code-Fragment cy bzw. cx, um Werte zwischen 1 und 15 für Vorwärtsfahrt bzw. -1 bis -15 für Rückwärtsfahrt zu erhalten. Für Kurvenfahrt addiere bzw. subtrahiere ich den halben cx-Wert. Noch nicht verwendet wird die letzte Stelle des Codes (0=Button nicht gedrückt, 1=Button gedrückt); damit könnte man Blinklicht einschalten, hupen oder zwischen Fernsteuerung und autonomen Modus umschalten.

Mein modifiziertes Programm läuft auf Anhieb und die Motoren drehen (eher zufällig) in die richtige Richtung. Wenn die Motoren in die falsche Richtung drehen, muss man entweder die Anschlüsse umpolen oder die Pinbelegung im Code ändern.

Hier der Code für das Robot Car zum Download:

 # Robot Car with Raspberry Pi Pico W BLE
 # Modified from Official Rasp Pi example here:
 # https://github.com/micropython/micropython/tree/master/examples/bluetooth
 # by Bernd54Albrecht for AZ-Delivery 2023
 
 
 import bluetooth
 import random
 import struct
 import time
 from ble_advertising import advertising_payload
 from micropython import const
 from machine import Pin, PWM
 
 
 ## define 3 fragments of the code, idle is 16160
 cy = 16     # forward/backward, digit 1 and 2
 cx = 16     # left/right, digit 3 and 4
 cb = 0      # button, digit 5 
 
 
 # Initialisation of blue LED
 led_blue = Pin(14, Pin.OUT)
 
 
 # Initialisation of motors
 vBatt = 6
 m1e = PWM(Pin(11))
 m11 = Pin(13,Pin.OUT)
 m12 = Pin(12,Pin.OUT)
 m2e = PWM(Pin(20))
 m21 = Pin(19,Pin.OUT)
 m22 = Pin(18,Pin.OUT)
 m1e.freq(1000)
 m2e.freq(1000)
 factor = 655.35 * 6/vBatt   # max PWM/100 * 6 / vBatt
 
 
 # self-defined function for 2 motors with PWM
 def motor(cy, cx):
     y = cy - 15   # forward/backward
     x = cx - 15   # left/right
     leftWheel = y + 0.5 * x
     rightWheel = y - 0.5 * x
     if leftWheel > 15:
         leftWheel = 15
     if leftWheel < -15:
         leftWheel = -15
     if rightWheel > 15:
         rightWheel = 15
     if rightWheel < -15:
         rightWheel = -15
     if leftWheel < -2:
         m11.off()
         m12.on()
     elif leftWheel > 2:
         m11.on()
         m12.off()
     else:
         m11.off()
         m12.off()
 #        m1e.duty_u16(0)
     if rightWheel < -2:
         m21.off()
         m22.on()
     elif rightWheel > 2:
         m21.on()
         m22.off()
     else:
         m21.off()
         m22.off()
 #        m2e.duty_u16(0)
     leftPWM = int(factor * (25 + 5*abs(leftWheel)))
     print("leftWheel = ",leftWheel," leftPWM = ", leftPWM)    
     rightPWM = int(factor * (25 + 5*abs(rightWheel)))
     print("rightWheel = ",rightWheel," rightPWM = ", rightPWM)    
     m1e.duty_u16(leftPWM)
     m2e.duty_u16(rightPWM)
 
 
 
 
 ## taken from ble_simple_peripheral.py
 _IRQ_CENTRAL_CONNECT = const(1)
 _IRQ_CENTRAL_DISCONNECT = const(2)
 _IRQ_GATTS_WRITE = const(3)
 
 
 _FLAG_READ = const(0x0002)
 _FLAG_WRITE_NO_RESPONSE = const(0x0004)
 _FLAG_WRITE = const(0x0008)
 _FLAG_NOTIFY = const(0x0010)
 
 
 _UART_UUID = bluetooth.UUID("6E400001-B5A3-F393-E0A9-E50E24DCCA9E")
 _UART_TX = (
     bluetooth.UUID("6E400003-B5A3-F393-E0A9-E50E24DCCA9E"),
     _FLAG_READ | _FLAG_NOTIFY,
 )
 _UART_RX = (
     bluetooth.UUID("6E400002-B5A3-F393-E0A9-E50E24DCCA9E"),
     _FLAG_WRITE | _FLAG_WRITE_NO_RESPONSE,
 )
 _UART_SERVICE = (
     _UART_UUID,
     (_UART_TX, _UART_RX),
 )
 
 
 
 
 class BLESimplePeripheral:
     def __init__(self, ble, name="mpy-uart"):
         self._ble = ble
         self._ble.active(True)
         self._ble.irq(self._irq)
         ((self._handle_tx, self._handle_rx),) = self._ble.gatts_register_services((_UART_SERVICE,))
         self._connections = set()
         self._write_callback = None
         self._payload = advertising_payload(name=name, services=[_UART_UUID])
         self._advertise()
 
 
     def _irq(self, event, data):
         # Track connections so we can send notifications.
         if event == _IRQ_CENTRAL_CONNECT:
             conn_handle, _, _ = data
             print("New connection", conn_handle)
             self._connections.add(conn_handle)
         elif event == _IRQ_CENTRAL_DISCONNECT:
             conn_handle, _, _ = data
             print("Disconnected", conn_handle)
             self._connections.remove(conn_handle)
             # Start advertising again to allow a new connection.
             self._advertise()
         elif event == _IRQ_GATTS_WRITE:
             conn_handle, value_handle = data
             value = self._ble.gatts_read(value_handle)
             if value_handle == self._handle_rx and self._write_callback:
                 self._write_callback(value)
 
 
     def send(self, data):
         for conn_handle in self._connections:
             self._ble.gatts_notify(conn_handle, self._handle_tx, data)
 
 
     def is_connected(self):
         return len(self._connections) > 0
 
 
     def _advertise(self, interval_us=500000):
         print("Starting advertising")
         self._ble.gap_advertise(interval_us, adv_data=self._payload)
 
 
     def on_write(self, callback):
         self._write_callback = callback
 
 
 # This is the MAIN LOOP
 def demo():    # This part modified to control Robot Car
     ble = bluetooth.BLE()
     p = BLESimplePeripheral(ble)
 
 
     def on_rx(code):  # code is what has been received
 
 
         code = int(code)
         cy = int(code/1000)             # digit 1 and 2
         cx = int((code-1000*cy)/10)     # digit 3 and 4
         cb = code - 1000*cy - 10*cx     # digit 5
         print("cy = ",cy," cx = ",cx," cb = ",cb)       # Print code fragments
         motor(cy,cx)       # call function motor with 2 parameters
 
 
         
     p.on_write(on_rx)
 
 
 
 
 if __name__ == "__main__":
     demo()

Der Programm-Code für den Controller beruht auf dem Beispielprogramm ble_simple_central.py. Hinzugefügt werden aus dem Modul machine die Klassen Pin und PWM, mit denen das Joystick-Modul initialisiert und abgefragt wird. Mit poti1 = ADC(26).read_u16() und poti2 = ADC(27).read_u16() werden die Zahlenwerte für die x- und y-Richtung zwischen 0 und 65535 ermittelt. Durch ganzzahlige Division durch 2048 erhält man den Wertebereich 0 bis 31. Die y-Werte werden mit 1000, die x-Werte mit 10 multipliziert, um die ersten 4 Stellen des fünfstelligen Codes zu erhalten. Mit der letzten Stelle wird der Zustand des Buttons addiert.

Beispiele: Code 16161 bedeutet Leerlauf und Button gedrückt,
Code 31160 bedeutet schnellste Geradeausfahrt und Button nicht gedrückt,
Code 25310 bedeutet moderate Geschwindigkeit und scharfe Rechtskurve,
Code 8080 bedeutet moderate Rückwärtsfahrt und leichte Linkskurve (die Null
am Anfang wird zwangsläufig weggelassen).

Dieser Code wird als String gesendet und im Programm des Robot Cars decodiert. Dazu wird er dort wieder in eine Integerzahl verwandelt und in die drei Fragmente zerlegt (s.o.)

Und hier der Code für den Controller zum Download:

 # PicoW_BLE_Robot_Controller.py
 # Joystick with two 10K potentiometers on ADC0 and ADC1
 # Modified from Official Rasp Pi example here:
 # https://github.com/micropython/micropython/tree/master/examples/bluetooth
 # by Bernd54Albrecht for AZ-Delivery 2023
 
 
 import bluetooth
 import random
 import struct
 import time
 import micropython
 from ble_advertising import decode_services, decode_name
 from micropython import const
 
 
 # Additional code for joystick
 from machine import ADC, Pin
 button = Pin(15, Pin.IN, Pin.PULL_UP)
 
 
 ## taken from ble_simple_central.py
 _IRQ_CENTRAL_CONNECT = const(1)
 _IRQ_CENTRAL_DISCONNECT = const(2)
 _IRQ_GATTS_WRITE = const(3)
 _IRQ_GATTS_READ_REQUEST = const(4)
 _IRQ_SCAN_RESULT = const(5)
 _IRQ_SCAN_DONE = const(6)
 _IRQ_PERIPHERAL_CONNECT = const(7)
 _IRQ_PERIPHERAL_DISCONNECT = const(8)
 _IRQ_GATTC_SERVICE_RESULT = const(9)
 _IRQ_GATTC_SERVICE_DONE = const(10)
 _IRQ_GATTC_CHARACTERISTIC_RESULT = const(11)
 _IRQ_GATTC_CHARACTERISTIC_DONE = const(12)
 _IRQ_GATTC_DESCRIPTOR_RESULT = const(13)
 _IRQ_GATTC_DESCRIPTOR_DONE = const(14)
 _IRQ_GATTC_READ_RESULT = const(15)
 _IRQ_GATTC_READ_DONE = const(16)
 _IRQ_GATTC_WRITE_DONE = const(17)
 _IRQ_GATTC_NOTIFY = const(18)
 _IRQ_GATTC_INDICATE = const(19)
 
 
 _ADV_IND = const(0x00)
 _ADV_DIRECT_IND = const(0x01)
 _ADV_SCAN_IND = const(0x02)
 _ADV_NONCONN_IND = const(0x03)
 
 
 _UART_SERVICE_UUID = bluetooth.UUID("6E400001-B5A3-F393-E0A9-E50E24DCCA9E")
 _UART_RX_CHAR_UUID = bluetooth.UUID("6E400002-B5A3-F393-E0A9-E50E24DCCA9E")
 _UART_TX_CHAR_UUID = bluetooth.UUID("6E400003-B5A3-F393-E0A9-E50E24DCCA9E")
 
 
 
 
 class BLESimpleCentral:
     def __init__(self, ble):
         self._ble = ble
         self._ble.active(True)
         self._ble.irq(self._irq)
 
 
         self._reset()
 
 
     def _reset(self):
         # Cached name and address from a successful scan.
         self._name = None
         self._addr_type = None
         self._addr = None
 
 
         # Callbacks for completion of various operations.
         # These reset back to None after being invoked.
         self._scan_callback = None
         self._conn_callback = None
         self._read_callback = None
 
 
         # Persistent callback for when new data is notified from the device.
         self._notify_callback = None
 
 
         # Connected device.
         self._conn_handle = None
         self._start_handle = None
         self._end_handle = None
         self._tx_handle = None
         self._rx_handle = None
 
 
     def _irq(self, event, data):
         if event == _IRQ_SCAN_RESULT:
             addr_type, addr, adv_type, rssi, adv_data = data
             if adv_type in (_ADV_IND, _ADV_DIRECT_IND) and _UART_SERVICE_UUID in decode_services(
                 adv_data
             ):
                 # Found a potential device, remember it and stop scanning.
                 self._addr_type = addr_type
                 self._addr = bytes(
                     addr
                 )  # Note: addr buffer is owned by caller so need to copy it.
                 self._name = decode_name(adv_data) or "?"
                 self._ble.gap_scan(None)
 
 
         elif event == _IRQ_SCAN_DONE:
             if self._scan_callback:
                 if self._addr:
                     # Found a device during the scan (and the scan was explicitly stopped).
                     self._scan_callback(self._addr_type, self._addr, self._name)
                     self._scan_callback = None
                 else:
                     # Scan timed out.
                     self._scan_callback(None, None, None)
 
 
         elif event == _IRQ_PERIPHERAL_CONNECT:
             # Connect successful.
             conn_handle, addr_type, addr = data
             if addr_type == self._addr_type and addr == self._addr:
                 self._conn_handle = conn_handle
                 self._ble.gattc_discover_services(self._conn_handle)
 
 
         elif event == _IRQ_PERIPHERAL_DISCONNECT:
             # Disconnect (either initiated by us or the remote end).
             conn_handle, _, _ = data
             if conn_handle == self._conn_handle:
                 # If it was initiated by us, it'll already be reset.
                 self._reset()
 
 
         elif event == _IRQ_GATTC_SERVICE_RESULT:
             # Connected device returned a service.
             conn_handle, start_handle, end_handle, uuid = data
             print("service", data)
             if conn_handle == self._conn_handle and uuid == _UART_SERVICE_UUID:
                 self._start_handle, self._end_handle = start_handle, end_handle
 
 
         elif event == _IRQ_GATTC_SERVICE_DONE:
             # Service query complete.
             if self._start_handle and self._end_handle:
                 self._ble.gattc_discover_characteristics(
                     self._conn_handle, self._start_handle, self._end_handle
                 )
             else:
                 print("Failed to find uart service.")
 
 
         elif event == _IRQ_GATTC_CHARACTERISTIC_RESULT:
             # Connected device returned a characteristic.
             conn_handle, def_handle, value_handle, properties, uuid = data
             if conn_handle == self._conn_handle and uuid == _UART_RX_CHAR_UUID:
                 self._rx_handle = value_handle
             if conn_handle == self._conn_handle and uuid == _UART_TX_CHAR_UUID:
                 self._tx_handle = value_handle
 
 
         elif event == _IRQ_GATTC_CHARACTERISTIC_DONE:
             # Characteristic query complete.
             if self._tx_handle is not None and self._rx_handle is not None:
                 # We've finished connecting and discovering device, fire the connect callback.
                 if self._conn_callback:
                     self._conn_callback()
             else:
                 print("Failed to find uart rx characteristic.")
 
 
         elif event == _IRQ_GATTC_WRITE_DONE:
             conn_handle, value_handle, status = data
             print("TX complete")
 
 
         elif event == _IRQ_GATTC_NOTIFY:
             conn_handle, value_handle, notify_data = data
             if conn_handle == self._conn_handle and value_handle == self._tx_handle:
                 if self._notify_callback:
                     self._notify_callback(notify_data)
 
 
     # Returns true if we've successfully connected and discovered characteristics.
     def is_connected(self):
         return (
             self._conn_handle is not None
             and self._tx_handle is not None
             and self._rx_handle is not None
         )
 
 
     # Find a device advertising the environmental sensor service.
     def scan(self, callback=None):
         self._addr_type = None
         self._addr = None
         self._scan_callback = callback
         self._ble.gap_scan(2000, 30000, 30000)
 
 
     # Connect to the specified device (otherwise use cached address from a scan).
     def connect(self, addr_type=None, addr=None, callback=None):
         self._addr_type = addr_type or self._addr_type
         self._addr = addr or self._addr
         self._conn_callback = callback
         if self._addr_type is None or self._addr is None:
             return False
         self._ble.gap_connect(self._addr_type, self._addr)
         return True
 
 
     # Disconnect from current device.
     def disconnect(self):
         if self._conn_handle is None:
             return
         self._ble.gap_disconnect(self._conn_handle)
         self._reset()
 
 
     # Send data over the UART
     def write(self, v, response=False):
         if not self.is_connected():
             return
         self._ble.gattc_write(self._conn_handle, self._rx_handle, v, 1 if response else 0)
 
 
     # Set handler for when data is received over the UART.
     def on_notify(self, callback):
         self._notify_callback = callback
 
 
 
 
 def demo(): # This is the MAIN LOOP
     ble = bluetooth.BLE()
     central = BLESimpleCentral(ble)
 
 
     not_found = False
 
 
     def on_scan(addr_type, addr, name):
         if addr_type is not None:
             print("Found peripheral:", addr_type, addr, name)
             central.connect()
         else:
             nonlocal not_found
             not_found = True
             print("No peripheral found.")
 
 
     central.scan(callback=on_scan)
 
 
     # Wait for connection...
     while not central.is_connected():
         time.sleep_ms(100)
         if not_found:
             return
 
 
     print("Connected")
 
 
     with_response = False
 
 
 # Modified section for joystick and calculation of code
     while central.is_connected():
         try:
             # Read the raw potentiometer value from specified potentiometer
             poti1 = ADC(26).read_u16()
             poti2 = ADC(27).read_u16()
             y = int(poti1/2048)* 1000
             x = int(poti2/2048)*10
             z = abs(button.value()-1)
             code = str(y + x + z)
             print("y = ", y)
             print("x = ", x)
             central.write(code, with_response)
             
         except:
             print("TX failed")
             time.sleep_ms(1000)
         time.sleep_ms(400 if with_response else 30)
 
 
     print("Disconnected")
 
 
 if __name__ == "__main__":
     demo()

Nach dem Testen am PC werden die beiden Programme jeweils unter dem Namen main.py auf den Pico Ws abgespeichert, um die Autostart-Funktion beim Betrieb mit Batterien zu aktivieren. Dabei muss der Pico W am Robot Car einige Sekunden vorher gestartet werden, um beim Einschalten des Controllers bereits den BLE-Dienst anzubieten.