Encoder based pose movement.

ch-13/2-displaying-a-pose/robot/code.py

@@ -19,6 +19,12 @@ def read_json():
       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
@@ -27,13 +33,10 @@ class Simulation:
               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.int16,
+          dtype=np.float,
       )
 
-def send_poses(samples):
-    send_json({
-        "poses": samples[:,:2].tolist(),
-    })
+
 
 async def command_handler(simulation):
     print("Starting handler")

ch-13/2.1-collision-avoidance/robot/code.py

@@ -59,7 +59,7 @@ def read_json():
 
 def send_poses(samples):
     send_json({
-        "poses": samples[:,:2].tolist(),
+        "poses": np.array(samples[:,:2], dtype=np.int16).tolist(),
     })
 
 
@@ -71,7 +71,7 @@ class Simulation:
               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.int16,
+          dtype=np.float,
       )
       self.distance_sensors = DistanceSensorTracker()
       self.collision_avoider = CollisionAvoid(self.distance_sensors)

ch-13/2.2-Moving with encoders/banana_shaped.py

@@ -0,0 +1,51 @@
+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))]])
+
+
+
+triangular_proportion = np.sqrt(6) / 2
+def get_triangular_sample(mean, standard_deviation):
+    base = triangular_proportion * (random.uniform(-standard_deviation, standard_deviation) + random.uniform(-standard_deviation, standard_deviation))
+    return mean + base
+
+
+motion_rotation = np.array([get_triangular_sample(motion_angle, 15) for _ in range(poses.shape[0])])
+motion_translation = np.array([get_triangular_sample(motion_scale, 10) 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()

ch-13/2.2-Moving with encoders/display_from_robot.py

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

ch-13/2.2-Moving with encoders/find_devices.py

@@ -0,0 +1,13 @@
+import asyncio
+import bleak
+
+async def run():
+    ble_uuid = "6E400001-B5A3-F393-E0A9-E50E24DCCA9E"
+    ble_name = "Adafruit Bluefruit LE"
+    devices = await bleak.BleakScanner.discover(service_uuids=[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==ble_name][0]
+    print(f"Found robot {ble_device_info.name}...")
+
+asyncio.run(run())

ch-13/2.2-Moving with encoders/pyproject.toml

@@ -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"

ch-13/2.2-Moving with encoders/requirements.txt

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

ch-13/2.2-Moving with encoders/robot/arena.py

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

ch-13/2.2-Moving with encoders/robot/code.py

@@ -0,0 +1,162 @@
+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 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()
+
+
+    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 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
+        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]])
+        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))

ch-13/2.2-Moving with encoders/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/2.2-Moving with encoders/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/2.2-Moving with encoders/robot/robot.py

@@ -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 = 70
+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

ch-13/2.2-Moving with encoders/robot_ble_connection.py

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