The robot moves - not sure the arrows face the right way.

ch-13/3-moving-poses-with-sensors/robot/code.py

@@ -0,0 +1,73 @@
+import asyncio
+import json
+import random
+from ulab import numpy as np
+
+import arena
+import robot
+
+class Simulation:
+  def __init__(self):
+    population_size = 10
+    self.poses = np.empty((population_size, 3), dtype=np.float)
+    for n in range(population_size):
+      self.poses[n] = random.uniform(0, arena.width), random.uniform(0, arena.height), random.uniform(0, 360)
+  
+  async def run(self):
+    for n in range(3):
+      encoder_left = robot.left_encoder.read()
+      encoder_right = robot.right_encoder.read()
+      robot.set_left(0.5)
+      robot.set_right(0.5)
+      await asyncio.sleep(0.1)
+      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
+      for pose in self.poses:
+        pose[0] += speed_in_mm * np.cos(np.radians(pose[2]))
+        pose[1] += speed_in_mm * np.sin(np.radians(pose[2]))
+        # pose[2] += robot.ticks_to_degrees(left_movement - right_movement) / robot.wheel_separation_mm
+        # pose[2] = pose[2] % 360
+    robot.stop()
+
+async def command_handler(simulation):
+  simulation_task = None
+  while True:
+    if robot.uart.in_waiting:
+      print("Receiving data...")
+      try:
+        data = robot.uart.readline().decode()
+      except UnicodeError:
+        print("Invalid data")
+        continue
+      try:
+        request = json.loads(data)
+        print(f"Received command: {request}")
+      except ValueError:
+        print("Invalid JSON")
+        continue
+      # {"command": "arena"}
+      if request["command"] == "arena":
+         response = {
+            "arena": arena.boundary_lines,
+            "target_zone": arena.target_zone,
+         }
+         robot.uart.write((json.dumps(response)+"\n").encode())
+      elif request["command"] == "start":
+        print("Starting simulation")
+        if simulation_task is None:
+          simulation_task = asyncio.create_task(simulation.run())
+      elif request["command"] == "stop":
+        robot.stop()
+        if simulation_task:
+          simulation_task.cancel()
+          simulation_task = None
+    else:
+      response = {
+        "poses": simulation.poses.tolist(),
+      }
+      robot.uart.write((json.dumps(response)+"\n").encode())
+    await asyncio.sleep(0.1)
+
+simulation= Simulation()
+asyncio.run(command_handler(simulation))