Movement with uncertainty
This commit is contained in:
Danny Staple
File diff suppressed because one or more lines are too long
@@ -8,7 +8,7 @@ import adafruit_bno055
|
||||
|
||||
uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
|
||||
|
||||
wheel_diameter_mm = 70
|
||||
wheel_diameter_mm = 69.5
|
||||
wheel_circumference_mm = math.pi * wheel_diameter_mm
|
||||
gear_ratio = 298
|
||||
encoder_poles = 28
|
||||
|
||||
File diff suppressed because one or more lines are too long
@@ -0,0 +1,50 @@
|
||||
import numpy as np
|
||||
from matplotlib import pyplot as plt, patches
|
||||
from robot import arena
|
||||
import random
|
||||
|
||||
fig, ax = plt.subplots()
|
||||
|
||||
# for line in arena.boundary_lines:
|
||||
# plt.plot([line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black")
|
||||
|
||||
poses = np.random.normal([250, 300], [2, 2], size=(200, 2))
|
||||
ax.scatter(poses[:, 0], poses[:, 1])
|
||||
|
||||
# create a line for a motion vector
|
||||
motion_angle = 30
|
||||
motion_scale = 300
|
||||
motion_line = np.array([[250, 300],
|
||||
[motion_scale * np.cos(np.radians(motion_angle)), motion_scale * np.sin(np.radians(motion_angle))]])
|
||||
|
||||
|
||||
def get_scaled_sample_around_mean(mean, scale):
|
||||
return mean + (random.uniform(-scale, scale) + random.uniform(-scale, scale)) / 2
|
||||
|
||||
rotation_scale = 0.05 * abs(motion_angle) + 0.01 * abs(motion_scale)
|
||||
translation_scale = 0.05 * abs(motion_scale) + 0.01 * abs(motion_angle)
|
||||
|
||||
motion_rotation = np.array([get_scaled_sample_around_mean(motion_angle, rotation_scale) for _ in range(poses.shape[0])])
|
||||
motion_translation = np.array([get_scaled_sample_around_mean(motion_scale, translation_scale) for _ in range(poses.shape[0])])
|
||||
|
||||
new_poses = np.zeros_like(poses)
|
||||
new_poses[:, 0] = poses[:, 0] + motion_translation * np.cos(np.radians(motion_rotation))
|
||||
new_poses[:, 1] = poses[:, 1] + motion_translation * np.sin(np.radians(motion_rotation))
|
||||
# new_poses[:, 0] = poses[:, 0] + motion_scale * np.cos(np.radians(motion_angle))
|
||||
# new_poses[:, 1] = poses[:, 1] + motion_scale * np.sin(np.radians(motion_angle))
|
||||
|
||||
ax.scatter(new_poses[:, 0], new_poses[:, 1])
|
||||
|
||||
# plot the vector arrow
|
||||
ax.arrow(*motion_line[0], *motion_line[1], color="red", width=5)
|
||||
|
||||
# plot the angles on top
|
||||
# line from original cluster middle, going east.
|
||||
ax.plot([250, 250+100], [300, 300], color="black")
|
||||
|
||||
angle = patches.Wedge((250, 300), 100, 0, motion_angle, width=20, color=(0.3, 0.3, 0.3, 0.3))
|
||||
ax.add_patch(angle)
|
||||
ax.text(370, 330, f"{motion_angle}°", horizontalalignment="center", verticalalignment="center",
|
||||
fontsize="x-large")
|
||||
|
||||
plt.show()
|
||||
@@ -0,0 +1,73 @@
|
||||
import asyncio
|
||||
import json
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.widgets import Button
|
||||
|
||||
from robot_ble_connection import BleConnection
|
||||
|
||||
|
||||
class RobotDisplay:
|
||||
def __init__(self):
|
||||
self.ble_connection = BleConnection(self.handle_data)
|
||||
self.buffer = ""
|
||||
self.arena = {}
|
||||
self.closed = False
|
||||
self.fig, self.ax = plt.subplots()
|
||||
self.poses = None
|
||||
|
||||
def handle_close(self, _):
|
||||
self.closed = True
|
||||
|
||||
def handle_data(self, data):
|
||||
self.buffer += data.decode("utf-8")
|
||||
while "\n" in self.buffer:
|
||||
line, self.buffer = self.buffer.split("\n", 1)
|
||||
print(f"Received data: {line}")
|
||||
try:
|
||||
message = json.loads(line)
|
||||
except ValueError:
|
||||
print("Error parsing JSON")
|
||||
return
|
||||
if "arena" in message:
|
||||
self.arena = message
|
||||
if "poses" in message:
|
||||
self.poses = np.array(message["poses"], dtype=np.int16)
|
||||
|
||||
def draw(self):
|
||||
self.ax.clear()
|
||||
if self.arena:
|
||||
for line in self.arena["arena"]:
|
||||
self.ax.plot(
|
||||
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black"
|
||||
)
|
||||
if self.poses is not None:
|
||||
self.ax.scatter(self.poses[:,0], self.poses[:,1], color="blue")
|
||||
|
||||
async def send_command(self, command):
|
||||
request = (json.dumps({"command": command}) ).encode()
|
||||
print(f"Sending request: {request}")
|
||||
await self.ble_connection.send_uart_data(request)
|
||||
|
||||
def start(self, _):
|
||||
self.button_task = asyncio.create_task(self.send_command("start"))
|
||||
|
||||
async def main(self):
|
||||
plt.ion()
|
||||
await self.ble_connection.connect()
|
||||
try:
|
||||
await self.send_command("arena")
|
||||
self.fig.canvas.mpl_connect("close_event", self.handle_close)
|
||||
start_button = Button(plt.axes([0.7, 0.05, 0.1, 0.075]), "Start")
|
||||
start_button.on_clicked(self.start)
|
||||
while not self.closed:
|
||||
self.draw()
|
||||
plt.draw()
|
||||
plt.pause(0.05)
|
||||
await asyncio.sleep(0.01)
|
||||
finally:
|
||||
await self.ble_connection.close()
|
||||
|
||||
|
||||
robot_display = RobotDisplay()
|
||||
asyncio.run(robot_display.main())
|
||||
+2572
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,22 @@
|
||||
[tool.poetry]
|
||||
name = "modelling-space"
|
||||
version = "0.1.0"
|
||||
description = ""
|
||||
authors = ["Danny Staple <danny@orionrobots.co.uk>"]
|
||||
readme = "README.md"
|
||||
packages = [{include = "modelling_space"}]
|
||||
|
||||
[tool.poetry.dependencies]
|
||||
python = "^3.9"
|
||||
matplotlib = "3.6.1"
|
||||
numpy = "1.23.4"
|
||||
bleak = "0.19.0"
|
||||
jupyter = "^1.0.0"
|
||||
|
||||
|
||||
[tool.poetry.group.dev.dependencies]
|
||||
black = "^22.10.0"
|
||||
|
||||
[build-system]
|
||||
requires = ["poetry-core"]
|
||||
build-backend = "poetry.core.masonry.api"
|
||||
@@ -0,0 +1,3 @@
|
||||
matplotlib==3.6.1
|
||||
numpy==1.23.4
|
||||
bleak==0.19.0
|
||||
@@ -0,0 +1,77 @@
|
||||
"""Represent the lines of the arena"""
|
||||
try:
|
||||
from ulab import numpy as np
|
||||
except ImportError:
|
||||
import numpy as np
|
||||
|
||||
boundary_lines = [
|
||||
[(0,0), (0, 1500)],
|
||||
[(0, 1500), (1500, 1500)],
|
||||
[(1500, 1500), (1500, 500)],
|
||||
[(1500, 500), (1000, 500)],
|
||||
[(1000, 500), (1000, 0)],
|
||||
[(1000, 0), (0, 0)],
|
||||
]
|
||||
|
||||
width = 1500
|
||||
height = 1500
|
||||
|
||||
|
||||
grid_cell_size = 50
|
||||
overscan = 10 # 10 each way
|
||||
|
||||
|
||||
def get_distance_to_segment(x, y, segment):
|
||||
"""Return the distance from the point to the segment.
|
||||
Segment -> ((x1, y1), (x2, y2))
|
||||
All segments are horizontal or vertical.
|
||||
"""
|
||||
x1, y1 = segment[0]
|
||||
x2, y2 = segment[1]
|
||||
# if the segment is horizontal, the point will be closest to the y value of the segment
|
||||
if y1 == y2 and x >= min(x1, x2) and x <= max(x1, x2):
|
||||
return abs(y - y1)
|
||||
# if the segment is vertical, the point will be closest to the x value of the segment
|
||||
if x1 == x2 and y >= min(y1, y2) and y <= max(y1, y2):
|
||||
return abs(x - x1)
|
||||
# the point will be closest to one of the end points
|
||||
return np.sqrt(
|
||||
min(
|
||||
(x - x1) ** 2 + (y - y1) ** 2,
|
||||
(x - x2) ** 2 + (y - y2) ** 2
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def get_distance_likelihood(x, y):
|
||||
"""Return the distance from the point to the nearest segment as a decay function."""
|
||||
min_distance = None
|
||||
for segment in boundary_lines:
|
||||
distance = get_distance_to_segment(x, y, segment)
|
||||
if min_distance is None or distance < min_distance:
|
||||
min_distance = distance
|
||||
return 1.0 / (1 + min_distance/250) ** 2
|
||||
|
||||
|
||||
grid_cell_size = 50
|
||||
overscan = 10 # 10 each way
|
||||
|
||||
# beam endpoint model
|
||||
def make_distance_grid():
|
||||
"""Take the boundary lines. With and overscan of 10 cells, and grid cell size of 5cm (50mm),
|
||||
make a grid of the distance to the nearest boundary line.
|
||||
"""
|
||||
grid = np.zeros((
|
||||
width // grid_cell_size + 2 * overscan,
|
||||
height // grid_cell_size + 2 * overscan
|
||||
), dtype=np.float)
|
||||
for x in range(grid.shape[0]):
|
||||
column_x = x * grid_cell_size - (overscan * grid_cell_size)
|
||||
for y in range(grid.shape[1]):
|
||||
row_y = y * grid_cell_size - (overscan * grid_cell_size)
|
||||
grid[x, y] = get_distance_likelihood(
|
||||
column_x, row_y
|
||||
)
|
||||
return grid
|
||||
|
||||
distance_grid = make_distance_grid()
|
||||
@@ -0,0 +1,182 @@
|
||||
import asyncio
|
||||
import json
|
||||
import random
|
||||
from ulab import numpy as np
|
||||
|
||||
import arena
|
||||
import robot
|
||||
|
||||
class DistanceSensorTracker:
|
||||
def __init__(self):
|
||||
robot.left_distance.distance_mode = 2
|
||||
robot.right_distance.distance_mode = 2
|
||||
robot.left_distance.timing_budget = 50
|
||||
robot.right_distance.timing_budget = 50
|
||||
self.left = 300
|
||||
self.right = 300
|
||||
|
||||
async def main(self):
|
||||
robot.left_distance.start_ranging()
|
||||
robot.right_distance.start_ranging()
|
||||
while True:
|
||||
if robot.left_distance.data_ready and robot.left_distance.distance:
|
||||
self.left = robot.left_distance.distance * 10 # convert to mm
|
||||
robot.left_distance.clear_interrupt()
|
||||
if robot.right_distance.data_ready and robot.right_distance.distance:
|
||||
self.right = robot.right_distance.distance * 10
|
||||
robot.right_distance.clear_interrupt()
|
||||
await asyncio.sleep(0.01)
|
||||
|
||||
|
||||
class CollisionAvoid:
|
||||
def __init__(self, distance_sensors):
|
||||
self.speed = 0.6
|
||||
self.distance_sensors = distance_sensors
|
||||
|
||||
async def main(self):
|
||||
while True:
|
||||
robot.set_right(self.speed)
|
||||
while self.distance_sensors.left < 300 or \
|
||||
self.distance_sensors.right < 300:
|
||||
robot.set_left(-self.speed)
|
||||
await asyncio.sleep(0.3)
|
||||
robot.set_left(self.speed)
|
||||
await asyncio.sleep(0)
|
||||
|
||||
def get_scaled_sample_around_mean(mean, scale):
|
||||
return mean + (random.uniform(-scale, scale) + random.uniform(-scale, scale)) / 2
|
||||
|
||||
def send_json(data):
|
||||
robot.uart.write((json.dumps(data) + "\n").encode())
|
||||
|
||||
def read_json():
|
||||
try:
|
||||
data = robot.uart.readline()
|
||||
decoded = data.decode()
|
||||
return json.loads(decoded)
|
||||
except (UnicodeError, ValueError):
|
||||
print("Invalid data")
|
||||
return None
|
||||
|
||||
|
||||
def send_poses(samples):
|
||||
send_json({
|
||||
"poses": np.array(samples[:,:2], dtype=np.int16).tolist(),
|
||||
})
|
||||
|
||||
|
||||
class Simulation:
|
||||
def __init__(self):
|
||||
self.population_size = 20
|
||||
self.poses = np.array(
|
||||
[(
|
||||
int(random.uniform(0, arena.width)),
|
||||
int(random.uniform(0, arena.height)),
|
||||
int(random.uniform(0, 360))) for _ in range(self.population_size)],
|
||||
dtype=np.float,
|
||||
)
|
||||
self.distance_sensors = DistanceSensorTracker()
|
||||
self.collision_avoider = CollisionAvoid(self.distance_sensors)
|
||||
self.last_encoder_left = robot.left_encoder.read()
|
||||
self.last_encoder_right = robot.right_encoder.read()
|
||||
self.alpha_rot = 0.05
|
||||
self.alpha_rot_trans = 0.01
|
||||
self.alpha_trans = 0.05
|
||||
self.alpha_trans_rot = 0.01
|
||||
|
||||
def convert_odometry_to_motion(self, left_encoder_delta, right_encoder_delta):
|
||||
"""
|
||||
left_encoder is the change in the left encoder
|
||||
right_encoder is the change in the right encoder
|
||||
returns rot1, trans, rot2
|
||||
rot1 is the rotation of the robot in degrees before the translation
|
||||
trans is the distance the robot has moved in mm
|
||||
rot2 is the rotation of the robot in degrees
|
||||
"""
|
||||
left_mm = left_encoder_delta * robot.ticks_to_mm
|
||||
right_mm = right_encoder_delta * robot.ticks_to_mm
|
||||
|
||||
if left_mm == right_mm:
|
||||
return 0, left_mm, 0
|
||||
|
||||
# calculate the radius of the arc
|
||||
radius = (robot.wheelbase_mm / 2) * (left_mm + right_mm) / (right_mm - left_mm)
|
||||
## angle = difference in steps / wheelbase
|
||||
d_theta = (right_mm - left_mm) / robot.wheelbase_mm
|
||||
# For a small enough motion, assume that the chord length = arc length
|
||||
arc_length = d_theta * radius
|
||||
rot1 = np.degrees(d_theta/2)
|
||||
rot2 = rot1
|
||||
return rot1, arc_length, rot2
|
||||
|
||||
def apply_motion_to_poses(self, rot1, trans, rot2):
|
||||
self.poses[:,2] += rot1
|
||||
rot1_radians = np.radians(self.poses[:,2])
|
||||
self.poses[:,0] += trans * np.cos(rot1_radians)
|
||||
self.poses[:,1] += trans * np.sin(rot1_radians)
|
||||
self.poses[:,2] += rot2
|
||||
self.poses[:,2] = np.array([float(theta % 360) for theta in self.poses[:,2]])
|
||||
|
||||
def apply_randomness_to_motion(self, rot1, trans, rot2):
|
||||
rot1_scale = self.alpha_rot * abs(rot1) + self.alpha_rot_trans * abs(trans)
|
||||
trans_scale = self.alpha_trans * abs(trans) + self.alpha_trans_rot * (abs(rot1) + abs(rot2))
|
||||
rot2_scale = self.alpha_rot * abs(rot2) + self.alpha_rot_trans * abs(trans)
|
||||
rot1_model = np.array([get_scaled_sample_around_mean(rot1, rot1_scale) for _ in range(self.poses.shape[0])])
|
||||
trans_model = np.array([get_scaled_sample_around_mean(trans, trans_scale) for _ in range(self.poses.shape[0])])
|
||||
rot2_model = np.array([get_scaled_sample_around_mean(rot2, rot2_scale) for _ in range(self.poses.shape[0])])
|
||||
return rot1_model, trans_model, rot2_model
|
||||
|
||||
def motion_model(self):
|
||||
"""Apply the motion model"""
|
||||
new_encoder_left = robot.left_encoder.read()
|
||||
new_encoder_right = robot.right_encoder.read()
|
||||
|
||||
rot1, trans, rot2 = self.convert_odometry_to_motion(
|
||||
new_encoder_left - self.last_encoder_left,
|
||||
new_encoder_right - self.last_encoder_right)
|
||||
self.last_encoder_left = new_encoder_left
|
||||
self.last_encoder_right = new_encoder_right
|
||||
|
||||
rot1_model, trans_model, rot2_model = self.apply_randomness_to_motion(rot1, trans, rot2)
|
||||
self.apply_motion_to_poses(rot1_model, trans_model, rot2_model)
|
||||
|
||||
print(
|
||||
json.dumps(
|
||||
[self.poses.tolist(), rot1, trans, rot2]
|
||||
)
|
||||
)
|
||||
|
||||
async def main(self):
|
||||
asyncio.create_task(self.distance_sensors.main())
|
||||
asyncio.create_task(self.collision_avoider.main())
|
||||
try:
|
||||
while True:
|
||||
send_poses(self.poses)
|
||||
await asyncio.sleep(0.05)
|
||||
self.motion_model()
|
||||
finally:
|
||||
robot.stop()
|
||||
|
||||
|
||||
async def command_handler(simulation):
|
||||
print("Starting handler")
|
||||
simulation_task = None
|
||||
while True:
|
||||
if robot.uart.in_waiting:
|
||||
request = read_json()
|
||||
if not request:
|
||||
continue
|
||||
print("Received: ", request)
|
||||
if request["command"] == "arena":
|
||||
send_json({
|
||||
"arena": arena.boundary_lines,
|
||||
})
|
||||
elif request["command"] == "start":
|
||||
if not simulation_task:
|
||||
simulation_task = asyncio.create_task(simulation.main())
|
||||
|
||||
await asyncio.sleep(0.1)
|
||||
|
||||
|
||||
simulation = Simulation()
|
||||
asyncio.run(command_handler(simulation))
|
||||
@@ -0,0 +1,27 @@
|
||||
class PIDController:
|
||||
def __init__(self, kp, ki, kd, d_filter_gain=0.1, imax=None, imin=None):
|
||||
self.kp = kp
|
||||
self.ki = ki
|
||||
self.kd = kd
|
||||
self.d_filter_gain = d_filter_gain
|
||||
self.imax = imax
|
||||
self.imin = imin
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
self.integral = 0
|
||||
self.error_prev = 0
|
||||
self.derivative = 0
|
||||
|
||||
def calculate(self, error, dt):
|
||||
self.integral += error * dt
|
||||
if self.imax is not None and self.integral > self.imax:
|
||||
self.integral = self.imax
|
||||
if self.imin is not None and self.integral < self.imin:
|
||||
self.integral = self.imin
|
||||
# Add a low pass filter to the difference
|
||||
difference = (error - self.error_prev) * self.d_filter_gain
|
||||
self.error_prev += difference
|
||||
self.derivative = difference / dt
|
||||
|
||||
return self.kp * error + self.ki * self.integral + self.kd * self.derivative
|
||||
@@ -0,0 +1,84 @@
|
||||
import rp2pio
|
||||
import adafruit_pioasm
|
||||
import array
|
||||
import asyncio
|
||||
|
||||
|
||||
program = """
|
||||
; use the osr for count
|
||||
; input pins c1 c2
|
||||
|
||||
set y, 0 ; clear y
|
||||
mov osr, y ; and clear osr
|
||||
read:
|
||||
; x will be the old value
|
||||
; y the new values
|
||||
mov x, y ; store old Y in x
|
||||
in null, 32 ; Clear ISR - using y
|
||||
in pins, 2 ; read two pins into y
|
||||
mov y, isr
|
||||
jmp x!=y, different ; Jump if its different
|
||||
jmp read ; otherwise loop back to read
|
||||
|
||||
different:
|
||||
; x has old value, y has new.
|
||||
; extract the upper bit of X.
|
||||
in x, 31 ; get bit 31 - old p1 (remember which direction it came in)
|
||||
in null, 31 ; keep only 1 bit
|
||||
mov x, isr ; put this back in x
|
||||
jmp !x, c1_old_zero
|
||||
|
||||
c1_old_not_zero:
|
||||
jmp pin, count_up
|
||||
jmp count_down
|
||||
|
||||
c1_old_zero:
|
||||
jmp pin, count_down
|
||||
; fall through
|
||||
count_up:
|
||||
; for a clockwise move - we'll add 1 by inverting
|
||||
mov x, ~ osr ; store inverted OSR on x
|
||||
jmp x--, fake ; use jump to take off 1
|
||||
fake:
|
||||
mov x, ~ x ; invert back
|
||||
jmp send
|
||||
count_down:
|
||||
; for a clockwise move, just take one off
|
||||
mov x, osr ; store osr in x
|
||||
jmp x--, send ; dec and send
|
||||
send:
|
||||
; send x.
|
||||
mov isr, x ; send it
|
||||
push noblock ; put ISR into input FIFO
|
||||
mov osr, x ; put X back in OSR
|
||||
jmp read ; loop back
|
||||
"""
|
||||
|
||||
assembled = adafruit_pioasm.assemble(program)
|
||||
|
||||
|
||||
class QuadratureEncoder:
|
||||
def __init__(self, first_pin, second_pin, reversed=False):
|
||||
"""Encoder with 2 pins. Must use sequential pins on the board"""
|
||||
self.sm = rp2pio.StateMachine(
|
||||
assembled,
|
||||
frequency=0,
|
||||
first_in_pin=first_pin,
|
||||
jmp_pin=second_pin,
|
||||
in_pin_count=2,
|
||||
)
|
||||
self.reversed = reversed
|
||||
self._buffer = array.array("i", [0])
|
||||
asyncio.create_task(self.poll_loop())
|
||||
|
||||
async def poll_loop(self):
|
||||
while True:
|
||||
await asyncio.sleep(0)
|
||||
while self.sm.in_waiting:
|
||||
self.sm.readinto(self._buffer)
|
||||
|
||||
def read(self):
|
||||
if self.reversed:
|
||||
return -self._buffer[0]
|
||||
else:
|
||||
return self._buffer[0]
|
||||
+76
@@ -0,0 +1,76 @@
|
||||
import board
|
||||
import pwmio
|
||||
import pio_encoder
|
||||
import busio
|
||||
import adafruit_vl53l1x
|
||||
import math
|
||||
import adafruit_bno055
|
||||
|
||||
uart = busio.UART(board.GP12, board.GP13, baudrate=9600)
|
||||
|
||||
wheel_diameter_mm = 69.5
|
||||
wheel_circumference_mm = math.pi * wheel_diameter_mm
|
||||
gear_ratio = 298
|
||||
encoder_poles = 28
|
||||
ticks_per_revolution = encoder_poles * gear_ratio
|
||||
ticks_to_mm = wheel_circumference_mm / ticks_per_revolution
|
||||
ticks_to_m = ticks_to_mm / 1000
|
||||
m_to_ticks = 1 / ticks_to_m
|
||||
wheelbase_mm = 170
|
||||
distance_sensor_from_middle = 40 # approx mm
|
||||
|
||||
motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)
|
||||
motor_A1 = pwmio.PWMOut(board.GP16, frequency=100)
|
||||
motor_B2 = pwmio.PWMOut(board.GP18, frequency=100)
|
||||
motor_B1 = pwmio.PWMOut(board.GP19, frequency=100)
|
||||
|
||||
right_motor = motor_A1, motor_A2
|
||||
left_motor = motor_B1, motor_B2
|
||||
|
||||
right_encoder = pio_encoder.QuadratureEncoder(board.GP20, board.GP21)
|
||||
left_encoder = pio_encoder.QuadratureEncoder(board.GP26, board.GP27, reversed=True)
|
||||
|
||||
i2c0 = busio.I2C(sda=board.GP0, scl=board.GP1)
|
||||
i2c1 = busio.I2C(sda=board.GP2, scl=board.GP3)
|
||||
|
||||
left_distance = adafruit_vl53l1x.VL53L1X(i2c0)
|
||||
right_distance = adafruit_vl53l1x.VL53L1X(i2c1)
|
||||
|
||||
imu = adafruit_bno055.BNO055_I2C(i2c0)
|
||||
|
||||
def stop():
|
||||
motor_A1.duty_cycle = 0
|
||||
motor_A2.duty_cycle = 0
|
||||
motor_B1.duty_cycle = 0
|
||||
motor_B2.duty_cycle = 0
|
||||
|
||||
|
||||
def set_speed(motor, speed):
|
||||
# Swap motor pins if we reverse the speed
|
||||
if abs(speed) < 0.1:
|
||||
motor[0].duty_cycle = 0
|
||||
motor[1].duty_cycle = 1
|
||||
return
|
||||
if speed < 0:
|
||||
direction = motor[1], motor[0]
|
||||
speed = -speed
|
||||
else:
|
||||
direction = motor
|
||||
speed = min(speed, 1) # limit to 1.0
|
||||
max_speed = 2 ** 16 - 1
|
||||
|
||||
direction[0].duty_cycle = int(max_speed * speed)
|
||||
direction[1].duty_cycle = 0
|
||||
|
||||
|
||||
def set_left(speed):
|
||||
set_speed(left_motor, speed)
|
||||
|
||||
|
||||
def set_right(speed):
|
||||
set_speed(right_motor, speed)
|
||||
|
||||
def check_imu_status():
|
||||
sys_status, gyro, accel, mag = imu.calibration_status
|
||||
uart.write(f"Sys: {sys_status}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}\n".encode())
|
||||
return sys_status == 3
|
||||
@@ -0,0 +1,38 @@
|
||||
import asyncio
|
||||
|
||||
import bleak
|
||||
|
||||
|
||||
class BleConnection:
|
||||
# See https://learn.adafruit.com/introducing-adafruit-ble-bluetooth-low-energy-friend/uart-service
|
||||
ble_uuid = "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
|
||||
rx_gatt = "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"
|
||||
tx_gatt = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
|
||||
ble_name = "Adafruit Bluefruit LE"
|
||||
|
||||
def __init__(self, receive_handler):
|
||||
self.ble_client = None
|
||||
self.receive_handler = receive_handler
|
||||
|
||||
def _uart_handler(self, _, data: bytes):
|
||||
self.receive_handler(data)
|
||||
|
||||
async def connect(self):
|
||||
print("Scanning for devices...")
|
||||
devices = await bleak.BleakScanner.discover(service_uuids=[self.ble_uuid])
|
||||
print(f"Found {len(devices)} devices")
|
||||
print([device.name for device in devices])
|
||||
ble_device_info = [device for device in devices if device.name==self.ble_name][0]
|
||||
print(f"Connecting to {ble_device_info.name}...")
|
||||
self.ble_client = bleak.BleakClient(ble_device_info.address)
|
||||
await self.ble_client.connect()
|
||||
print("Connected to {}".format(ble_device_info.name))
|
||||
self.notify_task = asyncio.create_task(
|
||||
self.ble_client.start_notify(self.rx_gatt, self._uart_handler)
|
||||
)
|
||||
|
||||
async def close(self):
|
||||
await self.ble_client.disconnect()
|
||||
|
||||
async def send_uart_data(self, data):
|
||||
await self.ble_client.write_gatt_char(self.tx_gatt, data)
|
||||
Reference in New Issue
Block a user