From df911dcfb0ae7a509337daa5e8b656352beb778b Mon Sep 17 00:00:00 2001
From: Drew Bednar <drew@androiddrew.com>
Date: Thu, 26 Aug 2021 17:35:04 -0400
Subject: [PATCH] working Arni controller

---
 bluetooth/esp32/arni/README.md                |  28 +++
 bluetooth/esp32/arni/ble_advertising.py       |  93 ++++++++++
 .../{ => arni}/ble_arni_odrive_controller.py  |  29 +++-
 bluetooth/esp32/arni/robot.py                 | 164 ++++++++++++++++++
 uart/odrive_uart.py                           |  90 ++++++++--
 5 files changed, 378 insertions(+), 26 deletions(-)
 create mode 100644 bluetooth/esp32/arni/README.md
 create mode 100644 bluetooth/esp32/arni/ble_advertising.py
 rename bluetooth/esp32/{ => arni}/ble_arni_odrive_controller.py (86%)
 create mode 100644 bluetooth/esp32/arni/robot.py

diff --git a/bluetooth/esp32/arni/README.md b/bluetooth/esp32/arni/README.md
new file mode 100644
index 0000000..98c778f
--- /dev/null
+++ b/bluetooth/esp32/arni/README.md
@@ -0,0 +1,28 @@
+# Arni
+
+This code supports using an ESP32 and the Dabble ios app to control 2 hoverboard motors
+driven by an Odrive.
+
+## Install
+
+```
+upy put  bluetooth/esp32/arni/ble_advertising.py ble_advertising.py
+upy put bluetooth/esp32/arni/robot.py robot.py
+upy put bluetooth/esp32/ble_arni_odrive_controller.py main.py
+```
+
+Confirm with `upy ls`
+
+## Usage
+
+With the ESP32 and Odrive powered on open the Dabble and connect. This
+does not support Joystick yet. This is a work in progress
+
+### Buttons
+
+|Key       |Action                                                   |
+|----------|---------------------------------------------------------|
+|Select    |Toggles Odrive idle / loop control states                |
+|Start     |Performs an Odrive AXIS_STATE_ENCODER_OFFSET_CALIBRATION |
+|          |                                                         |
+|          |                                                         |
\ No newline at end of file
diff --git a/bluetooth/esp32/arni/ble_advertising.py b/bluetooth/esp32/arni/ble_advertising.py
new file mode 100644
index 0000000..24433e7
--- /dev/null
+++ b/bluetooth/esp32/arni/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/arni/ble_arni_odrive_controller.py
similarity index 86%
rename from bluetooth/esp32/ble_arni_odrive_controller.py
rename to bluetooth/esp32/arni/ble_arni_odrive_controller.py
index a9c7514..65bb14c 100644
--- a/bluetooth/esp32/ble_arni_odrive_controller.py
+++ b/bluetooth/esp32/arni/ble_arni_odrive_controller.py
@@ -1,10 +1,13 @@
 # This module demonstrates uses the Nordic UART Service (NUS) to interpret gamepad
-
-
 import bluetooth
+from machine import UART
+from micropython import const
+
+# Arni
 from ble_advertising import advertising_payload
+from robot import ArniRobot, RobotController
 
-from micropython import const
+__version__ = "0.1.0"
 
 _IRQ_CENTRAL_CONNECT = const(1)
 _IRQ_CENTRAL_DISCONNECT = const(2)
@@ -97,9 +100,16 @@ class BLEUART:
         self._ble.gap_advertise(interval_us, adv_data=self._payload)
 
 
-def demo():
+
+
+def main():
     import time
 
+    robot_uart = UART(2, baudrate=115200, tx=17, rx=16)
+    arni = ArniRobot(robot_uart)
+    robot_controller = RobotController(arni)
+
+    # BT
     ble = bluetooth.BLE()
     uart = BLEUART(ble)
 
@@ -107,9 +117,12 @@ def demo():
         # Assumes Unicode text data is being sent.
         # print("rx: ", uart.read().decode().strip())
         data = uart.read()
-        print(type(data))
-        print(len(data))
-        print(data)
+        # print("Recieved {} bytes of data".format(data))
+        # for count, v in enumerate(data):
+        #     print("Index: {0} | Value: {1} | Binary: {2} | Hex: {3}".format(count,v,bin(v), hex(v)))
+        if len(data) == 8:
+            print("Recieved {} data".format(data))
+            robot_controller.interpret((data[5],data[6]))
 
     # Will execute this function on every received packet
     uart.irq(handler=on_rx)
@@ -126,4 +139,4 @@ def demo():
 
 
 if __name__ == "__main__":
-    demo()
\ No newline at end of file
+    main()
\ No newline at end of file
diff --git a/bluetooth/esp32/arni/robot.py b/bluetooth/esp32/arni/robot.py
new file mode 100644
index 0000000..2f433ac
--- /dev/null
+++ b/bluetooth/esp32/arni/robot.py
@@ -0,0 +1,164 @@
+import time
+# from machine import UART
+
+# ARn
+ARNI_INITIAL_VELOCITY = 0.5
+
+# ODrive Constants
+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
+
+# DABBLE BITS
+# BUTTONS
+DABBLE_START_BIT = 0
+DABBLE_SELECT_BIT = 1
+DABBLE_TRIANGLE_BIT = 2
+DABBLE_CIRCLE_BIT = 3
+DABBLE_CROSS_BIT = 4
+DABBLE_SQUARE_BIT = 5
+# ARROW PAD
+DABBLE_UP_BIT = 0
+DABBLE_DOWN_BIT = 1
+DABBLE_LEFT_BIT = 2
+DABBLE_RIGHT_BIT = 3
+
+# Globals
+# uart = UART(2, baudrate=115200, tx=17, rx=16)
+current_global_velocity = ARNI_INITIAL_VELOCITY  # Will be changed by controller during operation
+
+class RobotController:
+
+    def __init__(self, robot):
+        self.robot = robot
+
+    def interpret(self, dabble_bytes):
+        """
+        Interprets a tuples of bytes received from the Dabble BLE gamepad module.
+
+        Translates data into ArniRobot commands.
+
+        :param values: A two byte tuple
+            values[0] contains button data
+            values[1] contains arrow pad data
+        :return:
+        """
+        self._interpret_buttons(dabble_bytes[0])
+        self._interpret_arrows(dabble_bytes[1])
+
+    def _interpret_buttons(self, button_byte):
+        if button_byte & (1 << DABBLE_START_BIT):
+            self.robot.init_motors()
+        elif button_byte & (1 << DABBLE_SELECT_BIT):
+            if self.robot.active:
+                self.robot.set_idle()
+            else:
+                self.robot.set_active()
+
+    def _interpret_arrows(self, arrow_byte):
+        if arrow_byte & (1 << DABBLE_UP_BIT):
+            self.robot.forward()
+        elif arrow_byte & (1 << DABBLE_DOWN_BIT):
+            self.robot.reverse()
+        elif arrow_byte & (1 << DABBLE_LEFT_BIT):
+            self.robot.left()
+        elif arrow_byte & (1 << DABBLE_RIGHT_BIT):
+            self.robot.right()
+        else:
+            self.robot.stop()
+
+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')
+        """
+        self._uart.write("r {0}\n".format(value))
+        if not self._uart.any():
+            time.sleep_ms(100)
+        value = self._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
+        self._uart.write('w axis0.requested_state 7\nw axis1.requested_state 7\n')
+        self.initialized = True
+
+    def forward(self):
+        """Move forward."""
+        return self._uart.write('v 0 {0} 0\nv 1 -{0} 0\n'.format(self._vel))
+
+    def reverse(self):
+        """Reverse move."""
+        return self._uart.write('v 0 -{0} 0\nv 1 {0} 0\n'.format(self._vel))
+
+    def stop(self):
+        """All stop."""
+        return self._uart.write('v 0 0 0\nv 1 0 0\n')
+
+    def right(self):
+        """Hard turn right."""
+        return self._uart.write('v 0 0 0\nv 1 -{0} 0\n'.format(self._vel))
+
+    def left(self):
+        """Hard turn left."""
+        return self._uart.write('v 0 {0} 0\nv 1 0 0\n'.format(self._vel))
+
+    def right_forward(self):
+        """Right turn while moving forward."""
+        return self._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 self._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 self._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 self._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 self._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 self._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)
+# controller = RobotController(arni)
\ No newline at end of file
diff --git a/uart/odrive_uart.py b/uart/odrive_uart.py
index 3d1d862..fe1fe0c 100644
--- a/uart/odrive_uart.py
+++ b/uart/odrive_uart.py
@@ -1,10 +1,10 @@
 import time
 from machine import UART
 
-#
+# ARn
 ARNI_INITIAL_VELOCITY = 0.5
 
-#
+# ODrive Constants
 AXIS_STATE_UNDEFINED = 0
 AXIS_STATE_IDLE = 1
 AXIS_STATE_STARTUP_SEQUENCE = 2
@@ -15,10 +15,63 @@ AXIS_STATE_ENCODER_INDEX_SEARCH = 6
 AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7
 AXIS_STATE_CLOSED_LOOP_CONTROL = 8
 
+# DABBLE BITS
+# BUTTONS
+DABBLE_START_BIT = 0
+DABBLE_SELECT_BIT = 1
+DABBLE_TRIANGLE_BIT = 2
+DABBLE_CIRCLE_BIT = 3
+DABBLE_CROSS_BIT = 4
+DABBLE_SQUARE_BIT = 5
+# ARROW PAD
+DABBLE_UP_BIT = 0
+DABBLE_DOWN_BIT = 1
+DABBLE_LEFT_BIT = 2
+DABBLE_RIGHT_BIT = 3
+
 # Globals
 uart = UART(2, baudrate=115200, tx=17, rx=16)
 current_global_velocity = ARNI_INITIAL_VELOCITY  # Will be changed by controller during operation
 
+class RobotController:
+
+    def __init__(self, robot):
+        self.robot = robot
+
+    def interpret(self, dabble_bytes):
+        """
+        Interprets a tuples of bytes received from the Dabble BLE gamepad module.
+
+        Translates data into ArniRobot commands.
+
+        :param values: A two byte tuple
+            values[0] contains button data
+            values[1] contains arrow pad data
+        :return:
+        """
+        self._interpret_buttons(dabble_bytes[0])
+        self._interpret_arrows(dabble_bytes[1])
+
+    def _interpret_buttons(self, button_byte):
+        if button_byte & (1 << DABBLE_START_BIT):
+            self.robot.init_motors()
+        elif button_byte & (1 << DABBLE_SELECT_BIT):
+            if self.robot.active:
+                self.robot.set_idle()
+            else:
+                self.robot.set_active()
+
+    def _interpret_arrows(self, arrow_byte):
+        if arrow_byte & (1 << DABBLE_UP_BIT):
+            self.robot.forward()
+        elif arrow_byte & (1 << DABBLE_DOWN_BIT):
+            self.robot.reverse()
+        elif arrow_byte & (1 << DABBLE_LEFT_BIT):
+            self.robot.left()
+        elif arrow_byte & (1 << DABBLE_RIGHT_BIT):
+            self.robot.right()
+        else:
+            self.robot.stop()
 
 class ArniRobot:
 
@@ -35,10 +88,10 @@ class ArniRobot:
         Example:
             arni.get('vbus_voltage')
         """
-        uart.write("r {0}\n".format(value))
-        if not uart.any():
+        self._uart.write("r {0}\n".format(value))
+        if not self._uart.any():
             time.sleep_ms(100)
-        value = uart.read()
+        value = self._uart.read()
         if value:
             value = value.decode('ascii')
         return value or ''
@@ -49,50 +102,50 @@ class ArniRobot:
         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._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))
+        return self._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))
+        return self._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')
+        return self._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))
+        return self._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))
+        return self._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))
+        return self._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))
+        return self._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))
+        return self._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))
+        return self._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))
+        return self._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."""
@@ -100,11 +153,12 @@ class ArniRobot:
             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))
+        return self._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
+arni = ArniRobot(uart)
+controller = RobotController(arni)
\ No newline at end of file