diff --git a/ch-13/1-modelling-space/computer/display_arena_from_robot.py b/ch-13/1-modelling-space/computer/display_arena_from_robot.py new file mode 100644 index 0000000..0236def --- /dev/null +++ b/ch-13/1-modelling-space/computer/display_arena_from_robot.py @@ -0,0 +1,68 @@ +import asyncio +import json + +import matplotlib.pyplot as plt + +from robot_ble_connection import BleConnection + + +class RobotDisplay: + def __init__(self, ble_connection: BleConnection): + self.fig, self.ax = plt.subplots() + self.ble_connection = ble_connection + self.arena = None + self.display_closed = False + # handle closed event + self.fig.canvas.mpl_connect("close_event", self.handle_close) + ble_connection.receive_handler = self.handle_data + ble_connection.connected_handler = self.connected + self.line = "" + + def handle_close(self, _): + self.display_closed = True + + def handle_data(self, data): + line_part = data.decode("utf-8") + self.line += line_part + if not self.line.endswith("\n"): + return + print(f"Received data: {self.line}") + data = json.loads(self.line) + self.line = "" + if "arena" in data: + self.update(data) + + def update(self, arena): + self.arena = arena + self.ax.clear() + for line in arena["arena"]: + self.ax.plot([line[0][0], line[1][0]], + [line[0][1], line[1][1]], + color='black') + for line in arena["target_zone"]: + self.ax.plot([line[0][0], line[1][0]], + [line[0][1], line[1][1]], + color="red") + + def connected(self): + request = json.dumps({"command": "arena"}).encode() + print(f"Sending request for arena: {request}") + self.ble_connection.send_uart_data(request) + + +async def main(): + + plt.ion() + + ble_connection = BleConnection() + robot_display = RobotDisplay(ble_connection) + asyncio.create_task(ble_connection.connect()) + while not robot_display.display_closed: + plt.pause(0.05) + plt.draw() + await asyncio.sleep(0.01) + + plt.show() + + +asyncio.run(main()) diff --git a/ch-13/1-modelling-space/computer/find_devices.py b/ch-13/1-modelling-space/computer/find_devices.py new file mode 100644 index 0000000..ab7f96d --- /dev/null +++ b/ch-13/1-modelling-space/computer/find_devices.py @@ -0,0 +1,8 @@ +import asyncio +from robot_ble_connection import BleConnection + +async def run(): + ble_connection = BleConnection() + await ble_connection.connect() + +asyncio.run(run()) diff --git a/ch-13/1-modelling-space/computer/pyproject.toml b/ch-13/1-modelling-space/computer/pyproject.toml new file mode 100644 index 0000000..61aa084 --- /dev/null +++ b/ch-13/1-modelling-space/computer/pyproject.toml @@ -0,0 +1,15 @@ +[tool.poetry] +name = "modelling-space" +version = "0.1.0" +description = "" +authors = ["Danny Staple "] +readme = "README.md" +packages = [{include = "modelling_space"}] + +[tool.poetry.dependencies] +python = "^3.9" + + +[build-system] +requires = ["poetry-core"] +build-backend = "poetry.core.masonry.api" diff --git a/ch-13/1-modelling-space/computer/requirements.txt b/ch-13/1-modelling-space/computer/requirements.txt new file mode 100644 index 0000000..01fedac --- /dev/null +++ b/ch-13/1-modelling-space/computer/requirements.txt @@ -0,0 +1,3 @@ +matplotlib==3.6.1 +numpy==1.23.4 +bleak==0.19.0 diff --git a/ch-13/1-modelling-space/computer/robot_ble_connection.py b/ch-13/1-modelling-space/computer/robot_ble_connection.py new file mode 100644 index 0000000..0fbb72d --- /dev/null +++ b/ch-13/1-modelling-space/computer/robot_ble_connection.py @@ -0,0 +1,49 @@ +import asyncio +import typing + +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" + adafruit_rx_uuid = "6E400003-B5A3-F393-E0A9-E50E24DCCA9E" + adafruit_tx_uuid = "6E400002-B5A3-F393-E0A9-E50E24DCCA9E" + ble_name = "Adafruit Bluefruit LE" + + def __init__(self, receive_handler: typing.Callable[[bytes], None] = None, + connected_handler: typing.Callable[[], None] = None): + self.ble_client : bleak.BleakClient = None + # receive_handler(bytes) -> None + self.receive_handler = receive_handler + # connected_handler() -> None + self.connected_handler = connected_handler + + def _uart_handler(self, _, data: bytes): + # hmm - not receiving the whole message, only parts of it... + if self.receive_handler: + self.receive_handler(data) + + async def connect(self): + print("Scanning for devices...") + devices = await bleak.BleakScanner.discover(service_uuids=[self.ble_uuid]) + print("Found {} devices".format(len(devices))) + print([device.name for device in devices]) + ble_device_info = [device for device in devices if device.name==self.ble_name][0] + print("Connecting to {}...".format(ble_device_info.name)) + async with bleak.BleakClient(ble_device_info.address) as ble_client: + self.ble_client = ble_client + if self.connected_handler: + self.connected_handler() + print("Connected to {}".format(ble_device_info.name)) + asyncio.create_task( + self.ble_client.start_notify(self.adafruit_rx_uuid, self._uart_handler) + ) + while True: + await asyncio.sleep(1) + + def send_uart_data(self, data: bytes): + if self.ble_client: + asyncio.create_task( + self.ble_client.write_gatt_char(self.adafruit_tx_uuid, data) + ) diff --git a/ch-13/1-modelling-space/robot/arena.py b/ch-13/1-modelling-space/robot/arena.py new file mode 100644 index 0000000..17d4d98 --- /dev/null +++ b/ch-13/1-modelling-space/robot/arena.py @@ -0,0 +1,46 @@ +"""Represent the lines and target zone of the arena""" + + +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)], +] + + +target_zone = [ + [(1100, 900), (1100, 1100)], + [(1100, 1100), (1250, 1100)], + [(1250, 1100), (1250, 900)], + [(1250, 900), (1100, 900)], +] + +width = 1500 +height = 1500 + +def point_is_inside_arena(point): + """Return True if the point is inside the arena""" + # cheat a little, the arena is a rectangle, with a cutout. + # if the point is inside the rectangle, but not inside the cutout, it's inside the arena. + # this is far simpler than any line intersection method. + + # is it inside the rectangle? + if point[0] < 0 or point[0] > width \ + or point[1] < 0 or point[1] > height: + return False + # is it inside the cutout? + if point[0] > 1000 and point[1] < 500: + return False + return True + +def point_is_inside_target_zone(point): + """Return True if the point is inside the target zone""" + # cheat a little, the target zone is a rectangle. + # if the point is inside the rectangle, it's inside the target zone. + if point[0] < 1100 or point[0] > 1250 \ + or point[1] < 900 or point[1] > 1100: + return False + return True diff --git a/ch-13/1-modelling-space/robot/code.py b/ch-13/1-modelling-space/robot/code.py new file mode 100644 index 0000000..960da10 --- /dev/null +++ b/ch-13/1-modelling-space/robot/code.py @@ -0,0 +1,24 @@ +import asyncio +import json + +import arena +import robot + + +async def command_handler(): + while True: + if robot.uart.in_waiting: + print("Receiving data...") + data = robot.uart.readline().decode() + print(f"Received data: {data}") + request = json.loads(data) + print(f"Received command: {request}") + # {"command": "arena"} + if request["command"] == "arena": + response = { + "arena": arena.boundary_lines, + "target_zone": arena.target_zone, + } + robot.uart.write((json.dumps(response)+"\n").encode()) + +asyncio.run(command_handler()) diff --git a/ch-13/1-modelling-space/robot/pid_controller.py b/ch-13/1-modelling-space/robot/pid_controller.py new file mode 100644 index 0000000..a09358f --- /dev/null +++ b/ch-13/1-modelling-space/robot/pid_controller.py @@ -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 diff --git a/ch-13/1-modelling-space/robot/pio_encoder.py b/ch-13/1-modelling-space/robot/pio_encoder.py new file mode 100644 index 0000000..b831bf4 --- /dev/null +++ b/ch-13/1-modelling-space/robot/pio_encoder.py @@ -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] diff --git a/ch-13/1-modelling-space/robot/robot.py b/ch-13/1-modelling-space/robot/robot.py new file mode 100755 index 0000000..433d8ec --- /dev/null +++ b/ch-13/1-modelling-space/robot/robot.py @@ -0,0 +1,74 @@ +import board +import pwmio +import pio_encoder +import busio +import adafruit_vl53l1x +import math +import busio +import adafruit_bno055 + +uart = busio.UART(board.GP12, board.GP13, baudrate=9600) + +wheel_diameter_mm = 70 +wheel_circumference_mm = math.pi * wheel_diameter_mm +gear_ratio = 298 +encoder_poles = 28 +ticks_per_revolution = encoder_poles * gear_ratio +ticks_to_m = (wheel_circumference_mm / ticks_per_revolution) / 1000 +m_to_ticks = 1 / ticks_to_m + + +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 diff --git a/ch-13/simpler-arena.FCStd b/ch-13/simpler-arena.FCStd new file mode 100644 index 0000000..2b000e5 Binary files /dev/null and b/ch-13/simpler-arena.FCStd differ