Current state - before further changes

ch-13/8-seeking-target/robot/arena.py

@@ -0,0 +1,108 @@
+"""Represent the lines and target zone of the arena"""
+import math
+
+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
+
+def get_binary_occupancy_grid():
+  ## Convert the boundary lines to a grid map
+  ## with 50mm resolution
+  grid_size = 50
+  overscan_in_cells = 5
+  grid_width = int(width / grid_size) + 2 * overscan_in_cells
+  grid_height = int(height / grid_size) + 2 * overscan_in_cells
+  grid = [[0 for x in range(grid_width)] for y in range(grid_height)]
+  for scan_line in range(grid_height):
+    scan_y = (scan_line - overscan_in_cells) * grid_size
+    if scan_y < 0 or scan_y > height:
+      grid[scan_line] = [1 for x in range(grid_width)]
+      continue
+    # For each line, set the left overscan to 1
+    # and the right overscan to 1, but account for the cutout
+    grid[scan_line][0:overscan_in_cells] = [1 for x in range(overscan_in_cells)]
+    if scan_y < 500:
+      cutout_start = int(1000 / grid_size)
+      grid[scan_line][overscan_in_cells + cutout_start:] = [1 for x in range(grid_width - overscan_in_cells - cutout_start)]
+    else:
+      grid[scan_line][overscan_in_cells + int(width / grid_size):] = [1 for x in range(grid_width - overscan_in_cells - int(width / grid_size))]
+
+  return grid
+
+
+target_zone = [
+  [(1100, 900), (1100, 1100)],
+  [(1100, 1100), (1250, 1100)],
+  [(1250, 1100), (1250, 900)],
+  [(1250, 900), (1100, 900)],
+]
+target_zone_middle = (1175, 1000)
+
+
+def point_is_inside_arena(x, y):
+  """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 x < 0 or x > width \
+    or y < 0 or y > height:
+    return False
+  # is it inside the cutout?
+  if x > 1000 and y < 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
+
+def distance_from_line_segment(line_segment, point_x, point_y):
+  """Return the distance from the point to the line segment"""
+  # get the line as a, b, c where ax + by + c = 0
+  line_x1, line_y1 = line_segment[0]
+  line_x2, line_y2 = line_segment[1]
+  a = line_y1 - line_y2
+  b = line_x2 - line_x1
+  c = line_x1 * line_y2 - line_x2 * line_y1
+  # calculate the distance
+  return abs(a * point_x + b * point_y + c) / math.sqrt(a * a + b * b)
+
+
+def point_near_boundaries(point_x, point_y, distance):
+  """Return True if the point is close enough to the boundary lines"""
+  for line_segment in boundary_lines:
+    if distance_from_line_segment(line_segment, point_x, point_y) < distance:
+      return True
+  return False
+
+def heading_for_target_zone_middle(point_x, point_y):
+  """Return the heading to the middle of the target zone"""
+  # get the heading to the middle of the target zone
+  heading = math.atan2(target_zone_middle[1] - point_y, target_zone_middle[0] - point_x)
+  # convert to degrees
+  heading = math.degrees(heading)
+  # convert to compass heading
+  heading = 90 - heading
+  # convert to 0-360
+  if heading < 0:
+    heading += 360
+  return heading
+
+def distance_to_target_zone_middle(point_x, point_y):
+  """Return the distance to the middle of the target zone"""
+  return math.sqrt((target_zone_middle[0] - point_x) ** 2 + (target_zone_middle[1] - point_y) ** 2)

ch-13/8-seeking-target/robot/code.py

@@ -0,0 +1,272 @@
+import asyncio
+import json
+from guassian import get_gaussian_sample
+from ulab import numpy as np
+
+import arena
+import robot
+
+
+class Simulation:
+    def __init__(self):
+        self.population_size = 50
+        self.left_distance = 100
+        self.right_distance = 100
+
+        # poses can be a list of x[pop size], y[pop size], heading[pop size]
+        # while less "pythonic" it is more "numpyish"
+        self.poses = np.empty((3, 0), dtype=np.float)
+        self.regenerate_poses()
+        self.mean = np.array((arena.width / 2, arena.height / 2, 180), dtype=np.float)
+        self.std = np.array((arena.width / 4, arena.height / 4, 180), dtype=np.float)
+
+    def regenerate_poses(self):
+        # determine the number
+        number_to_make = self.population_size - len(self.poses[0])
+        new_poses = np.empty((3, number_to_make), dtype=np.float)
+        # determine the mean and std for new poses
+        if len(self.poses[0]) > 0:
+            self.mean = np.mean(self.poses, 0)
+            self.std = np.std(self.poses, 0)
+        else:
+            self.mean = np.array((arena.width / 2, arena.height / 2, 180))
+            self.std = np.array((arena.width / 4, arena.height / 4, 180))
+        print("mean.shape :", mean.shape)
+        print("std.shape :", std.shape)
+
+        # generate new poses
+        print(f"Generating {number_to_make} new poses.")
+        for n in range(number_to_make):
+            new_poses[0, n] = get_gaussian_sample(self.mean[0], self.std[0])
+            new_poses[1, n] = get_gaussian_sample(self.mean[1], self.std[1])
+            new_poses[2, n] = get_gaussian_sample(self.mean[2], self.std[2])
+        # set poses to concatenation of new poses and remaining poses
+        self.poses = np.concatenate((self.poses, new_poses), axis=1)
+
+    # hmm - cannot expand, or resize np arrays.
+    # maybe we use normal py arrays apart from the mean/std bit?
+    # a real numpy whizz may have a better way.
+    
+    
+
+    async def move_robot(self):
+        starting_heading = robot.imu.euler[0]
+        encoder_left = robot.left_encoder.read()
+        encoder_right = robot.right_encoder.read()
+        robot.set_left(0.8)
+        robot.set_right(0.8)
+        await asyncio.sleep(0.1)
+        # record sensor changes
+        left_movement = robot.left_encoder.read() - encoder_left
+        right_movement = robot.right_encoder.read() - encoder_right
+        speed_in_mm = robot.ticks_to_m * ((left_movement + right_movement) / 2) * 1000
+        new_heading = robot.imu.euler[0]
+        if new_heading:
+          heading_change = starting_heading - new_heading
+        else:
+          print("Failed to get heading")
+          heading_change = 0
+
+        # move poses
+        radians = np.radians(self.poses[2])
+        self.poses[0] += speed_in_mm * np.cos(radians)
+        self.poses[1] += speed_in_mm * np.sin(radians)
+        self.poses[2] += np.full(self.poses[2].shape, heading_change)
+        self.poses[2] = np.vectorize(lambda n: n % 360)(self.poses[2])
+
+    # ```
+    # >>> first = np.array([[1, 2, 3, 4], [5,6,7,8]])
+    # >>> second = np.array([3, 6, 9, 12])
+    # >>> first + second
+    # array([[ 4,  8, 12, 16],
+    #        [ 8, 12, 16, 20]])
+    # >>> second = np.array([[3, 6, 9, 12], [1,1,1,1]])
+    # >>> first + second
+    # array([[ 4,  8, 12, 16],
+    #        [ 6,  7,  8,  9]])
+    # >>> test=np.arange(10)
+    # >>> test
+    # array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9], dtype=int16)
+    # >>> mask = np.empty(len(test), dtype=np.bool)
+    # >>> mask
+    # array([False, False, False, False, False, False, False, False, False], dtype=bool)
+    # >>> mask[3] = [True]
+    # >>> mask[5] = [True]
+    # >>> mask[6] = [True]
+    # >>> test[mask]
+    # array([4, 6, 7], dtype=int16)
+    # ```
+
+    def eliminate_poses(self, left_distance, right_distance):
+        poses_to_keep = np.empty(len(self.poses[0]), dtype=np.bool)
+        # first eliminate those outside the arena
+        for position, pose in enumerate(self.poses.transpose()):
+            # first those outside the arena
+            poses_to_keep[position] = [arena.point_is_inside_arena(pose[0], pose[1])]
+        # apply the keep as a mask to the poses using ulab.
+        self.poses = np.array(
+            [
+                self.poses[0][poses_to_keep],
+                self.poses[1][poses_to_keep],
+                self.poses[2][poses_to_keep],
+            ]
+        )
+
+        # Then deal with sensors
+        # todo: would reorganising these pose arrays let us better use ulab tools?
+        distance_sensors = np.empty((4, len(self.poses[0])), dtype=np.float)
+        # sensors - they are facing forward, either side of the robot. Project them out to the sides
+        distance_sensors[0] = self.poses[
+            0
+        ] + robot.distance_sensor_from_middle * np.cos(np.radians(self.poses[2] + 90))
+        distance_sensors[1] = self.poses[
+            1
+        ] + robot.distance_sensor_from_middle * np.sin(np.radians(self.poses[2] + 90))
+        # sensor right
+        distance_sensors[2] = self.poses[
+            0
+        ] + robot.distance_sensor_from_middle * np.cos(np.radians(self.poses[2] - 90))
+        distance_sensors[3] = self.poses[
+            1
+        ] + robot.distance_sensor_from_middle * np.sin(np.radians(self.poses[2] - 90))
+        # now project these sensors forward based on their distance read
+        distance_sensors[0] += np.cos(np.radians(self.poses[2])) * left_distance
+        distance_sensors[1] += np.sin(np.radians(self.poses[2])) * left_distance
+        distance_sensors[2] += np.cos(np.radians(self.poses[2])) * right_distance
+        distance_sensors[3] += np.sin(np.radians(self.poses[2])) * right_distance
+        # now eliminate those sensors that are too far outside the boundary
+        # extension (extra layer) - those too far inside the boundary
+        # extension (if needed) - make the boundary fuzzier - 20cm error?
+        # poses to keep must be redefined - it's now shorter
+        poses_to_keep = np.empty(len(self.poses[0]), dtype=np.bool)
+        boundary_error = 100 # mm
+        for position, distance_sensor in enumerate(distance_sensors.transpose()):
+            poses_to_keep[position] = [
+                arena.point_near_boundaries(distance_sensor[0], distance_sensor[1], boundary_error)
+                and arena.point_near_boundaries(distance_sensor[2], distance_sensor[3], boundary_error)
+            ]
+        # apply the keep as a mask to the poses using ulab.
+        self.poses = np.array(
+            [
+                self.poses[0][poses_to_keep],
+                self.poses[1][poses_to_keep],
+                self.poses[2][poses_to_keep],
+            ]
+        )
+        print(self.poses.shape)
+
+    # Helpful note - when debugging this numpy code, use prints with the serial console
+    # printing the object shape is often a very helpful way to debug what is going on.
+    # Eg:
+    #    print("poses_to_keep.shape:", poses_to_keep.shape)
+    #    print("self.poses[0].shape:", self.poses[0].shape)
+
+    # steps:
+    # regenerate poses
+    # move robot
+    # eliminate poses
+    # send poses
+    async def distance_sensor_updater(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_distance = 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_distance = robot.right_distance.distance * 10
+                robot.right_distance.clear_interrupt()
+            await asyncio.sleep(0.1)
+
+    async def run(self):
+        asyncio.create_task(self.distance_sensor_updater())
+        try:
+            for _ in range(15):
+                self.regenerate_poses()
+                await self.move_robot()
+                self.eliminate_poses(self.left_distance, self.right_distance)
+        finally:
+            robot.stop()
+
+
+def send_json(data):
+    robot.uart.write((json.dumps(data) + "\n").encode())
+
+
+def read_command():
+    data = robot.uart.readline()
+    try:
+        decoded = data.decode()
+    except UnicodeError:
+        print("UnicodeError decoding :")
+        print(data)
+        return None
+    try:
+        request = json.loads(decoded)
+    except ValueError:
+        print("ValueError reading json from:")
+        print(decoded)
+        return None
+    return request
+
+
+async def updater(simulation):
+    print("starting updater")
+    while True:
+        sys_status, gyro, accel, mag = robot.imu.calibration_status
+        if sys_status < 3:
+            send_json(
+                {
+                    "imu_calibration": {
+                        "gyro": gyro,
+                        "accel": accel,
+                        "mag": mag,
+                        "sys": sys_status,
+                    }
+                }
+            )
+        send_json(
+            {
+                "poses": simulation.poses.transpose().tolist(),
+                "std": simulation.std.tolist(),
+                "mean": simulation.mean.tolist(),
+            }
+        )
+        await asyncio.sleep(0.5)
+
+
+async def command_handler(simulation):
+    update_task = asyncio.create_task(updater(simulation))
+    print("Starting handler")
+    simulation_task = None
+    # This line - helpful to debug - rapid iteration on connected robot.
+    # simulation_task = asyncio.create_task(simulation.run())
+    while True:
+        if robot.uart.in_waiting:
+            print("Receiving data...")
+            request = read_command()
+            if not request:
+                print("no request")
+                continue
+            # {"command": "arena"}
+            if request["command"] == "arena":
+                send_json(
+                    {
+                        "arena": arena.boundary_lines,
+                        "target_zone": arena.target_zone,
+                    }
+                )
+            elif request["command"] == "start":
+                print("Starting simulation")
+                if simulation_task is None or simulation_task.done():
+                    simulation_task = asyncio.create_task(simulation.run())
+            elif request["command"] == "stop":
+                robot.stop()
+                if simulation_task and not simulation_task.done():
+                    simulation_task.cancel()
+                    simulation_task = None
+        await asyncio.sleep(0.1)
+
+
+simulation = Simulation()
+asyncio.run(command_handler(simulation))

ch-13/8-seeking-target/robot/guassian.py

@@ -0,0 +1,15 @@
+import random
+import math
+
+def get_standard_normal_sample():
+    """Using the Marasaglia Polar method"""
+    while True:
+        u = random.uniform(-1, 1)
+        v = random.uniform(-1, 1)
+        s = u * u + v * v
+        if s >= 1:
+            continue
+        return u * math.sqrt(-2 * math.log(s) / s)
+
+def get_gaussian_sample(mean, standard_deviation):
+    return get_standard_normal_sample() * standard_deviation + mean

ch-13/8-seeking-target/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/8-seeking-target/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/8-seeking-target/robot/robot.py

@@ -0,0 +1,78 @@
+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)
+
+distance_sensor_from_middle = 40 # approx mm
+
+imu = adafruit_bno055.BNO055_I2C(i2c0)
+imu.mode = adafruit_bno055.NDOF_MODE # should be in chapter 12!
+
+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