Review comments

- Indentation issues
- Inconsistencies

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

@@ -11,12 +11,12 @@ def send_json(data):
 
 def read_json():
     try:
-      data = robot.uart.readline()
+        data = robot.uart.readline()
-      decoded = data.decode()
+        decoded = data.decode()
-      return json.loads(decoded)
+        return json.loads(decoded)
     except (UnicodeError, ValueError):
-      print("Invalid data")
+        print("Invalid data")
-      return None
+        return None
 
 
 def send_poses(samples):
@@ -27,14 +27,14 @@ def send_poses(samples):
 
 class Simulation:
     def __init__(self):
-      self.population_size = 20
+        self.population_size = 20
-      self.poses = np.array(
+        self.poses = np.array(
-          [(
+            [(
-              int(random.uniform(0, arena.width)),
+                int(random.uniform(0, arena.width)),
-              int(random.uniform(0, arena.height)),
+                int(random.uniform(0, arena.height)),
-              int(random.uniform(0, 360))) for _ in range(self.population_size)],
+                int(random.uniform(0, 360))) for _ in range(self.population_size)],
-          dtype=np.float,
+            dtype=np.float,
-      )
+        )
 
 
 

ch-13/3.2-collision-avoidance/display_from_robot.py

@@ -2,6 +2,7 @@ import asyncio
 import json
 import numpy as np
 import matplotlib.pyplot as plt
+from matplotlib.widgets import Button
 
 from robot_ble_connection import BleConnection
 
@@ -43,15 +44,22 @@ class RobotDisplay:
         if self.poses is not None:
             self.axes.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:
-            request = json.dumps({"command": "arena"}).encode()
+            await self.send_command("arena")
-            print(f"Sending request for arena: {request}")
-            await self.ble_connection.send_uart_data(request)
             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()

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

@@ -47,12 +47,12 @@ def send_json(data):
 
 def read_json():
     try:
-      data = robot.uart.readline()
+        data = robot.uart.readline()
-      decoded = data.decode()
+        decoded = data.decode()
-      return json.loads(decoded)
+        return json.loads(decoded)
     except (UnicodeError, ValueError):
-      print("Invalid data")
+        print("Invalid data")
-      return None
+        return None
 
 
 def send_poses(samples):
@@ -63,16 +63,16 @@ def send_poses(samples):
 
 class Simulation:
     def __init__(self):
-      self.population_size = 20
+        self.population_size = 20
-      self.poses = np.array(
+        self.poses = np.array(
-          [(
+            [(
-              int(random.uniform(0, arena.width)),
+                int(random.uniform(0, arena.width)),
-              int(random.uniform(0, arena.height)),
+                int(random.uniform(0, arena.height)),
-              int(random.uniform(0, 360))) for _ in range(self.population_size)],
+                int(random.uniform(0, 360))) for _ in range(self.population_size)],
-          dtype=np.float,
+            dtype=np.float,
-      )
+        )
-      self.distance_sensors = DistanceSensorTracker()
+        self.distance_sensors = DistanceSensorTracker()
-      self.collision_avoider = CollisionAvoid(self.distance_sensors)
+        self.collision_avoider = CollisionAvoid(self.distance_sensors)
 
     async def main(self):
         asyncio.create_task(self.distance_sensors.main())

ch-13/3.3-moving-with-encoders/robot/code.py

@@ -47,12 +47,12 @@ def send_json(data):
 
 def read_json():
     try:
-      data = robot.uart.readline()
+        data = robot.uart.readline()
-      decoded = data.decode()
+        decoded = data.decode()
-      return json.loads(decoded)
+        return json.loads(decoded)
     except (UnicodeError, ValueError):
-      print("Invalid data")
+        print("Invalid data")
-      return None
+        return None
 
 
 def send_poses(samples):

ch-13/3.4-movement-uncertainty/display_from_robot.py

@@ -20,7 +20,7 @@ class RobotDisplay:
         self.closed = True
 
     def handle_data(self, data):
-        self.buffer += data.decode)
+        self.buffer += data.decode()
         while "\n" in self.buffer:
             line, self.buffer = self.buffer.split("\n", 1)
             print(f"Received data: {line}")

ch-13/3.4-movement-uncertainty/robot/code.py

@@ -49,12 +49,12 @@ def send_json(data):
 
 def read_json():
     try:
-      data = robot.uart.readline()
+        data = robot.uart.readline()
-      decoded = data.decode()
+        decoded = data.decode()
-      return json.loads(decoded)
+        return json.loads(decoded)
     except (UnicodeError, ValueError):
-      print("Invalid data")
+        print("Invalid data")
-      return None
+        return None
 
 
 def send_poses(samples):

ch-13/4.1-generating-weights/robot/code.py

@@ -49,12 +49,12 @@ def send_json(data):
 
 def read_json():
     try:
-      data = robot.uart.readline()
+        data = robot.uart.readline()
-      decoded = data.decode()
+        decoded = data.decode()
-      return json.loads(decoded)
+        return json.loads(decoded)
     except (UnicodeError, ValueError):
-      print("Invalid data")
+        print("Invalid data")
-      return None
+        return None
 
 
 def send_poses(samples):
@@ -77,10 +77,10 @@ class Simulation:
         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 = 0.09
-        self.alpha_rot_trans = 0.01
+        self.alpha_rot_trans = 0.05
-        self.alpha_trans = 0.05
+        self.alpha_trans = 0.12
-        self.alpha_trans_rot = 0.01
+        self.alpha_trans_rot = 0.05
 
     def convert_odometry_to_motion(self, left_encoder_delta, right_encoder_delta):
         """

ch-13/4.3-monte-carlo/robot/code.py

@@ -49,12 +49,12 @@ def send_json(data):
 
 def read_json():
     try:
-      data = robot.uart.readline()
+        data = robot.uart.readline()
-      decoded = data.decode()
+        decoded = data.decode()
-      return json.loads(decoded)
+        return json.loads(decoded)
     except (UnicodeError, ValueError):
-      print("Invalid data")
+        print("Invalid data")
-      return None
+        return None
 
 
 def send_poses(samples):
@@ -192,6 +192,7 @@ class Simulation:
             while weight_index > cumulative_weights:
                 source_index += 1
                 cumulative_weights += weights[source_index]
+                source_index = min(len(weights) - 1, source_index)
             samples[current_index] = self.poses[source_index]
         return samples
     async def main(self):

ch-13/4.3-monte-carlo_perf/robot/code.py

@@ -56,12 +56,12 @@ def send_json(data):
 
 def read_json():
     try:
-      data = robot.uart.readline()
+        data = robot.uart.readline()
-      decoded = data.decode()
+        decoded = data.decode()
-      return json.loads(decoded)
+        return json.loads(decoded)
     except (UnicodeError, ValueError):
-      print("Invalid data")
+        print("Invalid data")
-      return None
+        return None
 
 
 def send_poses(samples):

ch-13/4.3.1-test-dist-sensors/code.py

@@ -0,0 +1,42 @@
+import time
+import json
+import robot
+import asyncio
+
+def send_json(data):
+    robot.send_line(json.dumps(data))
+
+class DistanceSensorTracker:
+    def __init__(self):
+        robot.left_distance.distance_mode = 2
+        robot.right_distance.distance_mode = 2
+        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)
+
+async def distance_reporter(sensors: DistanceSensorTracker):
+    while True:
+        data = {
+            "distance": {
+                "left": sensors.left, 
+                "right": sensors.right
+            }
+        }
+        print(data)
+        send_json(data)
+        await asyncio.sleep(0.1)
+
+distance_sensors = DistanceSensorTracker()
+asyncio.create_task(distance_sensors.main())
+asyncio.run(distance_reporter(distance_sensors))

ch-13/4.3.1-test-dist-sensors/robot.py

@@ -0,0 +1,80 @@
+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, timeout=0.1)
+
+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
+dist_side_mm = 37 # approx mm
+dist_forward_mm = 66 # 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 send_line(message):
+    uart.write(f"{message}\n".encode())
+
+def check_imu_status():
+  sys_status, gyro, accel, mag = imu.calibration_status
+  send_line(f"Sys: {sys_status}, Gyro: {gyro}, Accel: {accel}, Mag: {mag}")
+  return sys_status == 3