From 259ea914ff5399aba7e01bdc6853cc6a9fc4a8f1 Mon Sep 17 00:00:00 2001
From: Drew Bednar <drew@androiddrew.com>
Date: Tue, 24 Aug 2021 15:21:44 -0400
Subject: [PATCH] some bluetooth and odrive updates

---
 bluetooth/esp32/ble_advertising.py            |  93 +++++++++++++
 bluetooth/esp32/ble_arni_odrive_controller.py | 129 +++++++++++++++++
 bluetooth/esp32/ble_uart_peripheral.py        | 131 ++++++++++++++++++
 bluetooth/zs_040/README.md                    |   3 +-
 motors/pwm/README.md                          |   3 +-
 uart/odrive_uart.py                           | 110 +++++++++++++++
 6 files changed, 467 insertions(+), 2 deletions(-)
 create mode 100644 bluetooth/esp32/ble_advertising.py
 create mode 100644 bluetooth/esp32/ble_arni_odrive_controller.py
 create mode 100644 bluetooth/esp32/ble_uart_peripheral.py
 create mode 100644 uart/odrive_uart.py

diff --git a/bluetooth/esp32/ble_advertising.py b/bluetooth/esp32/ble_advertising.py
new file mode 100644
index 0000000..24433e7
--- /dev/null
+++ b/bluetooth/esp32/ble_advertising.py
@@ -0,0 +1,93 @@
+# Helpers for generating BLE advertising payloads.
+
+from micropython import const
+import struct
+import bluetooth
+
+# Advertising payloads are repeated packets of the following form:
+#   1 byte data length (N + 1)
+#   1 byte type (see constants below)
+#   N bytes type-specific data
+
+_ADV_TYPE_FLAGS = const(0x01)
+_ADV_TYPE_NAME = const(0x09)
+_ADV_TYPE_UUID16_COMPLETE = const(0x3)
+_ADV_TYPE_UUID32_COMPLETE = const(0x5)
+_ADV_TYPE_UUID128_COMPLETE = const(0x7)
+_ADV_TYPE_UUID16_MORE = const(0x2)
+_ADV_TYPE_UUID32_MORE = const(0x4)
+_ADV_TYPE_UUID128_MORE = const(0x6)
+_ADV_TYPE_APPEARANCE = const(0x19)
+
+
+# Generate a payload to be passed to gap_advertise(adv_data=...).
+def advertising_payload(limited_disc=False, br_edr=False, name=None, services=None, appearance=0):
+    payload = bytearray()
+
+    def _append(adv_type, value):
+        nonlocal payload
+        payload += struct.pack("BB", len(value) + 1, adv_type) + value
+
+    _append(
+        _ADV_TYPE_FLAGS,
+        struct.pack("B", (0x01 if limited_disc else 0x02) + (0x18 if br_edr else 0x04)),
+    )
+
+    if name:
+        _append(_ADV_TYPE_NAME, name)
+
+    if services:
+        for uuid in services:
+            b = bytes(uuid)
+            if len(b) == 2:
+                _append(_ADV_TYPE_UUID16_COMPLETE, b)
+            elif len(b) == 4:
+                _append(_ADV_TYPE_UUID32_COMPLETE, b)
+            elif len(b) == 16:
+                _append(_ADV_TYPE_UUID128_COMPLETE, b)
+
+    # See org.bluetooth.characteristic.gap.appearance.xml
+    if appearance:
+        _append(_ADV_TYPE_APPEARANCE, struct.pack("<h", appearance))
+
+    return payload
+
+
+def decode_field(payload, adv_type):
+    i = 0
+    result = []
+    while i + 1 < len(payload):
+        if payload[i + 1] == adv_type:
+            result.append(payload[i + 2 : i + payload[i] + 1])
+        i += 1 + payload[i]
+    return result
+
+
+def decode_name(payload):
+    n = decode_field(payload, _ADV_TYPE_NAME)
+    return str(n[0], "utf-8") if n else ""
+
+
+def decode_services(payload):
+    services = []
+    for u in decode_field(payload, _ADV_TYPE_UUID16_COMPLETE):
+        services.append(bluetooth.UUID(struct.unpack("<h", u)[0]))
+    for u in decode_field(payload, _ADV_TYPE_UUID32_COMPLETE):
+        services.append(bluetooth.UUID(struct.unpack("<d", u)[0]))
+    for u in decode_field(payload, _ADV_TYPE_UUID128_COMPLETE):
+        services.append(bluetooth.UUID(u))
+    return services
+
+
+def demo():
+    payload = advertising_payload(
+        name="micropython esp32",
+        services=[bluetooth.UUID(0x181A), bluetooth.UUID("6E400001-B5A3-F393-E0A9-E50E24DCCA9E")],
+    )
+    print(payload)
+    print(decode_name(payload))
+    print(decode_services(payload))
+
+
+if __name__ == "__main__":
+    demo()
\ No newline at end of file
diff --git a/bluetooth/esp32/ble_arni_odrive_controller.py b/bluetooth/esp32/ble_arni_odrive_controller.py
new file mode 100644
index 0000000..a9c7514
--- /dev/null
+++ b/bluetooth/esp32/ble_arni_odrive_controller.py
@@ -0,0 +1,129 @@
+# This module demonstrates uses the Nordic UART Service (NUS) to interpret gamepad
+
+
+import bluetooth
+from ble_advertising import advertising_payload
+
+from micropython import const
+
+_IRQ_CENTRAL_CONNECT = const(1)
+_IRQ_CENTRAL_DISCONNECT = const(2)
+_IRQ_GATTS_WRITE = const(3)
+
+_FLAG_WRITE = const(0x0008)
+_FLAG_NOTIFY = const(0x0010)
+
+
+# The 128-bit vendor-specific service UUID is 6E400001-B5A3-F393-E0A9-E50E24DCCA9E (16-bit offset: 0x0001).
+_UART_UUID = bluetooth.UUID("6E400001-B5A3-F393-E0A9-E50E24DCCA9E")
+# Defines the UART_TX characteristic
+# Notify
+_UART_TX = (
+    bluetooth.UUID("6E400003-B5A3-F393-E0A9-E50E24DCCA9E"),
+    _FLAG_NOTIFY,
+)
+# Defines the UART_RX characteristic
+# Write or Write Without Response
+# Writes data to the RX Characteristic to send it on to the UART interface.
+_UART_RX = (
+    bluetooth.UUID("6E400002-B5A3-F393-E0A9-E50E24DCCA9E"),
+    _FLAG_WRITE,
+)
+_UART_SERVICE = (
+    _UART_UUID,
+    (_UART_TX, _UART_RX),
+)
+
+# org.bluetooth.characteristic.gap.appearance.xml
+_ADV_APPEARANCE_GENERIC_COMPUTER = const(128)
+
+
+class BLEUART:
+    def __init__(self, ble, name="arni-robot", rxbuf=100):
+        self._ble = ble
+        self._ble.active(True)
+        self._ble.irq(self._irq)
+        ((self._tx_handle, self._rx_handle),) = self._ble.gatts_register_services((_UART_SERVICE,))
+        # Increase the size of the rx buffer and enable append mode.
+        self._ble.gatts_set_buffer(self._rx_handle, rxbuf, True)
+        self._connections = set()
+        self._rx_buffer = bytearray()
+        self._handler = None
+        # Optionally add services=[_UART_UUID], but this is likely to make the payload too large.
+        self._payload = advertising_payload(name=name, appearance=_ADV_APPEARANCE_GENERIC_COMPUTER)
+        self._advertise()
+
+    def irq(self, handler):
+        self._handler = handler
+
+    def _irq(self, event, data):
+        # Track connections so we can send notifications.
+        if event == _IRQ_CENTRAL_CONNECT:
+            conn_handle, _, _ = data
+            self._connections.add(conn_handle)
+        elif event == _IRQ_CENTRAL_DISCONNECT:
+            conn_handle, _, _ = data
+            if conn_handle in self._connections:
+                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
+            if conn_handle in self._connections and value_handle == self._rx_handle:
+                self._rx_buffer += self._ble.gatts_read(self._rx_handle)
+                if self._handler:
+                    self._handler()
+
+    def any(self):
+        return len(self._rx_buffer)
+
+    def read(self, sz=None):
+        if not sz:
+            sz = len(self._rx_buffer)
+        result = self._rx_buffer[0:sz]
+        self._rx_buffer = self._rx_buffer[sz:]
+        return result
+
+    def write(self, data):
+        for conn_handle in self._connections:
+            self._ble.gatts_notify(conn_handle, self._tx_handle, data)
+
+    def close(self):
+        for conn_handle in self._connections:
+            self._ble.gap_disconnect(conn_handle)
+        self._connections.clear()
+
+    def _advertise(self, interval_us=500000):
+        self._ble.gap_advertise(interval_us, adv_data=self._payload)
+
+
+def demo():
+    import time
+
+    ble = bluetooth.BLE()
+    uart = BLEUART(ble)
+
+    def on_rx():
+        # Assumes Unicode text data is being sent.
+        # print("rx: ", uart.read().decode().strip())
+        data = uart.read()
+        print(type(data))
+        print(len(data))
+        print(data)
+
+    # Will execute this function on every received packet
+    uart.irq(handler=on_rx)
+
+    try:
+        print("Starting BLE Nordic UART Service.")
+        while True:
+            # Just keeps the program alive
+            time.sleep_ms(1000)
+    except KeyboardInterrupt:
+        pass
+
+    uart.close()
+
+
+if __name__ == "__main__":
+    demo()
\ No newline at end of file
diff --git a/bluetooth/esp32/ble_uart_peripheral.py b/bluetooth/esp32/ble_uart_peripheral.py
new file mode 100644
index 0000000..08c6de3
--- /dev/null
+++ b/bluetooth/esp32/ble_uart_peripheral.py
@@ -0,0 +1,131 @@
+# This example demonstrates a peripheral implementing the Nordic UART Service (NUS).
+# The Bluetooth LE GATT Nordic UART Service is a custom service that receives and writes data and serves as a bridge
+# to the UART interface.
+
+import bluetooth
+from ble_advertising import advertising_payload
+
+from micropython import const
+
+_IRQ_CENTRAL_CONNECT = const(1)
+_IRQ_CENTRAL_DISCONNECT = const(2)
+_IRQ_GATTS_WRITE = const(3)
+
+_FLAG_WRITE = const(0x0008)
+_FLAG_NOTIFY = const(0x0010)
+
+
+# The 128-bit vendor-specific service UUID is 6E400001-B5A3-F393-E0A9-E50E24DCCA9E (16-bit offset: 0x0001).
+_UART_UUID = bluetooth.UUID("6E400001-B5A3-F393-E0A9-E50E24DCCA9E")
+# Defines the UART_TX characteristic
+# Notify
+_UART_TX = (
+    bluetooth.UUID("6E400003-B5A3-F393-E0A9-E50E24DCCA9E"),
+    _FLAG_NOTIFY,
+)
+# Defines the UART_RX characteristic
+# Write or Write Without Response
+# Writes data to the RX Characteristic to send it on to the UART interface.
+_UART_RX = (
+    bluetooth.UUID("6E400002-B5A3-F393-E0A9-E50E24DCCA9E"),
+    _FLAG_WRITE,
+)
+_UART_SERVICE = (
+    _UART_UUID,
+    (_UART_TX, _UART_RX),
+)
+
+# org.bluetooth.characteristic.gap.appearance.xml
+_ADV_APPEARANCE_GENERIC_COMPUTER = const(128)
+
+
+class BLEUART:
+    def __init__(self, ble, name="mpy-uart", rxbuf=100):
+        self._ble = ble
+        self._ble.active(True)
+        self._ble.irq(self._irq)
+        ((self._tx_handle, self._rx_handle),) = self._ble.gatts_register_services((_UART_SERVICE,))
+        # Increase the size of the rx buffer and enable append mode.
+        self._ble.gatts_set_buffer(self._rx_handle, rxbuf, True)
+        self._connections = set()
+        self._rx_buffer = bytearray()
+        self._handler = None
+        # Optionally add services=[_UART_UUID], but this is likely to make the payload too large.
+        self._payload = advertising_payload(name=name, appearance=_ADV_APPEARANCE_GENERIC_COMPUTER)
+        self._advertise()
+
+    def irq(self, handler):
+        self._handler = handler
+
+    def _irq(self, event, data):
+        # Track connections so we can send notifications.
+        if event == _IRQ_CENTRAL_CONNECT:
+            conn_handle, _, _ = data
+            self._connections.add(conn_handle)
+        elif event == _IRQ_CENTRAL_DISCONNECT:
+            conn_handle, _, _ = data
+            if conn_handle in self._connections:
+                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
+            if conn_handle in self._connections and value_handle == self._rx_handle:
+                self._rx_buffer += self._ble.gatts_read(self._rx_handle)
+                if self._handler:
+                    self._handler()
+
+    def any(self):
+        return len(self._rx_buffer)
+
+    def read(self, sz=None):
+        if not sz:
+            sz = len(self._rx_buffer)
+        result = self._rx_buffer[0:sz]
+        self._rx_buffer = self._rx_buffer[sz:]
+        return result
+
+    def write(self, data):
+        for conn_handle in self._connections:
+            self._ble.gatts_notify(conn_handle, self._tx_handle, data)
+
+    def close(self):
+        for conn_handle in self._connections:
+            self._ble.gap_disconnect(conn_handle)
+        self._connections.clear()
+
+    def _advertise(self, interval_us=500000):
+        self._ble.gap_advertise(interval_us, adv_data=self._payload)
+
+
+def demo():
+    import time
+
+    ble = bluetooth.BLE()
+    uart = BLEUART(ble)
+
+    def on_rx():
+        # Assumes Unicode text data is being sent.
+        print("rx: ", uart.read().decode().strip())
+
+    # Will execute this function on every received packet
+    uart.irq(handler=on_rx)
+    nums = [4, 8, 15, 16, 23, 42]
+    i = 0
+
+    try:
+        print("Starting BLE Nordic UART Service.")
+        while True:
+            # Demonstrates the Uart write characteristic. Loops through the nums list
+            # transmitting one value per second.
+            uart.write(str(nums[i]) + "\n")
+            i = (i + 1) % len(nums)
+            time.sleep_ms(1000)
+    except KeyboardInterrupt:
+        pass
+
+    uart.close()
+
+
+if __name__ == "__main__":
+    demo()
\ No newline at end of file
diff --git a/bluetooth/zs_040/README.md b/bluetooth/zs_040/README.md
index 2dde71f..587af2e 100644
--- a/bluetooth/zs_040/README.md
+++ b/bluetooth/zs_040/README.md
@@ -6,4 +6,5 @@
 - https://protosupplies.com/product/hc-05-zs-040-bluetooth-module/
 - https://docs.micropython.org/en/latest/esp32/quickref.html#uart-serial-bus
 - https://learn.adafruit.com/introducing-the-adafruit-bluefruit-le-uart-friend/command-mode-1
-- https://learn.sparkfun.com/tutorials/serial-communication/rules-of-serial
\ No newline at end of file
+- https://learn.sparkfun.com/tutorials/serial-communication/rules-of-serial
+- https://learn.sparkfun.com/tutorials/terminal-basics/basic-terminology-
\ No newline at end of file
diff --git a/motors/pwm/README.md b/motors/pwm/README.md
index f06a246..8aa4b4e 100644
--- a/motors/pwm/README.md
+++ b/motors/pwm/README.md
@@ -2,4 +2,5 @@
 
 ## Resources
 
-- [PWM L298N Article](https://howtomechatronics.com/tutorials/arduino/arduino-dc-motor-control-tutorial-l298n-pwm-h-bridge/)
\ No newline at end of file
+- [PWM L298N Article](https://howtomechatronics.com/tutorials/arduino/arduino-dc-motor-control-tutorial-l298n-pwm-h-bridge/)
+- https://out-of-cheese-error.netlify.app/l298n-with-esp8266
\ No newline at end of file
diff --git a/uart/odrive_uart.py b/uart/odrive_uart.py
new file mode 100644
index 0000000..3d1d862
--- /dev/null
+++ b/uart/odrive_uart.py
@@ -0,0 +1,110 @@
+import time
+from machine import UART
+
+#
+ARNI_INITIAL_VELOCITY = 0.5
+
+#
+AXIS_STATE_UNDEFINED = 0
+AXIS_STATE_IDLE = 1
+AXIS_STATE_STARTUP_SEQUENCE = 2
+AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3
+AXIS_STATE_MOTOR_CALIBRATION = 4
+AXIS_STATE_SENSORLESS_CONTROL = 5
+AXIS_STATE_ENCODER_INDEX_SEARCH = 6
+AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7
+AXIS_STATE_CLOSED_LOOP_CONTROL = 8
+
+# Globals
+uart = UART(2, baudrate=115200, tx=17, rx=16)
+current_global_velocity = ARNI_INITIAL_VELOCITY  # Will be changed by controller during operation
+
+
+class ArniRobot:
+
+    def __init__(self, uart, vel=None):
+        self._uart = uart
+        self._vel = vel if vel else ARNI_INITIAL_VELOCITY
+        self.active = False
+        self.initialized = False
+
+
+    def get(self, value):
+        """Read value from odrive config.
+
+        Example:
+            arni.get('vbus_voltage')
+        """
+        uart.write("r {0}\n".format(value))
+        if not uart.any():
+            time.sleep_ms(100)
+        value = uart.read()
+        if value:
+            value = value.decode('ascii')
+        return value or ''
+
+    def init_motors(self):
+        """Runs index ops and sets control loop.
+
+        Must be run with the motors not under load.
+        """
+        # AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7
+        uart.write('w axis0.requested_state 7\nw axis1.requested_state 7\n')
+        self.initialized = True
+
+    def forward(self):
+        """Move forward."""
+        return uart.write('v 0 {0} 0\nv 1 -{0} 0\n'.format(self._vel))
+
+    def reverse(self):
+        """Reverse move."""
+        return uart.write('v 0 -{0} 0\nv 1 {0} 0\n'.format(self._vel))
+
+    def stop(self):
+        """All stop."""
+        return uart.write('v 0 0 0\nv 1 0 0\n')
+
+    def right(self):
+        """Hard turn right."""
+        return uart.write('v 0 0 0\nv 1 -{0} 0\n'.format(self._vel))
+
+    def left(self):
+        """Hard turn left."""
+        return uart.write('v 0 {0} 0\nv 1 0 0\n'.format(self._vel))
+
+    def right_forward(self):
+        """Right turn while moving forward."""
+        return uart.write('v 0 {0} 0\nv 1 -{1} 0\n'.format(self._vel / 2, self._vel))
+
+    def right_reverse(self):
+        """Right turn while moving in reverse."""
+        return uart.write('v 0 -{0} 0\nv 1 {1} 0\n'.format(self._vel / 2, self._vel))
+
+    def left_forward(self):
+        """Left turn while moving forward."""
+        return uart.write('v 0 {0} 0\nv 1 -{1} 0\n'.format(self._vel, self._vel / 2))
+
+    def left_reverse(self):
+        """Right turn while moving in reverse."""
+        return uart.write('v 0 -{0} 0\nv 1 {0} 0\n'.format(self._vel, self._vel / 2))
+
+    def set_idle(self):
+        """Set motor control to idle."""
+        self.stop()
+        self.active = False
+        return uart.write('w axis0.requested_state {0})\nw axis1.requested_state {0}\n'.format(AXIS_STATE_IDLE))
+
+    def set_active(self):
+        """Set motor control to closed loop control."""
+        if not self.initialized:
+            print("Robot motors have not been initialized. Initialize motors before entering control loop.")
+
+        self.active = True
+        return uart.write('w axis0.requested_state {0})\nw axis1.requested_state {0}\n'.format(AXIS_STATE_CLOSED_LOOP_CONTROL))
+
+    def set_velocity(self, velocity):
+        """Set motor velocity."""
+        self._vel = velocity
+        return self._vel
+
+arni = ArniRobot(uart)
\ No newline at end of file