some bluetooth and odrive updates
Drew Bednar
3 years ago
parent
f6262ba212
commit
259ea914ff
@ -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,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()
|
||||
@ -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()
|
||||
@ -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)
|
||||
Loading…
Reference in New Issue