working Arni controller
Drew Bednar
3 years ago
parent
259ea914ff
commit
df911dcfb0
@ -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 |
|
||||
| | |
|
||||
| | |
|
||||
@ -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()
|
||||
@ -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)
|
||||
Loading…
Reference in New Issue