Chapter 13 modelling space initial.

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())

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())

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 <danny@orionrobots.co.uk>"]
+readme = "README.md"
+packages = [{include = "modelling_space"}]
+
+[tool.poetry.dependencies]
+python = "^3.9"
+
+
+[build-system]
+requires = ["poetry-core"]
+build-backend = "poetry.core.masonry.api"

ch-13/1-modelling-space/computer/requirements.txt

@@ -0,0 +1,3 @@
+matplotlib==3.6.1
+numpy==1.23.4
+bleak==0.19.0

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)
+            )

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

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())

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

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]

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