From 389f2f768a1ff02243ef6a7d53ac904b1fb980e3 Mon Sep 17 00:00:00 2001 From: Danny Staple Date: Tue, 6 Dec 2022 21:32:47 +0000 Subject: [PATCH] Display working. Sensors starting to work. Clusters are forming. --- .../computer/display_from_robot.py | 22 +++---- ch-13/full-version/robot/code.py | 57 ++++++++++++------- 2 files changed, 44 insertions(+), 35 deletions(-) diff --git a/ch-13/full-version/computer/display_from_robot.py b/ch-13/full-version/computer/display_from_robot.py index 11e4b76..22f45a2 100644 --- a/ch-13/full-version/computer/display_from_robot.py +++ b/ch-13/full-version/computer/display_from_robot.py @@ -14,17 +14,18 @@ class RobotDisplay: self.arena = {} self.display_closed = False self.fig, self.ax = plt.subplots() - self.pose_coords = [] - self.pose_uv = [] + self.poses = np.zeros([200, 3], dtype=np.float32) def handle_close(self, _): self.display_closed = True def handle_data(self, data): self.line += data.decode("utf-8") + # print(f"Received data: {data.decode('utf-8')}") + # print(f"Line is now: {self.line}") while "\n" in self.line: line, self.line = self.line.split("\n", 1) - # print(f"Received data: {line}") + print(f"Received line: {line}") try: message = json.loads(line) except ValueError: @@ -33,12 +34,8 @@ class RobotDisplay: if "arena" in message: self.arena = message if "poses" in message: - # the robot poses are an array of [x, y, theta] arrays. - # matplotlib quiver plots wants an [x] ,[y] and [angle] arrays - poses = np.array(message["poses"]) - self.pose_coords = poses[:,0:1].T - angle_rads = np.deg2rad(poses[:,2]) - self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)]) + print(message) + self.poses[message["offset"]: message["offset"] + len(message["poses"])] = message["poses"] def draw(self): self.ax.clear() @@ -47,12 +44,7 @@ class RobotDisplay: self.ax.plot( [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black" ) - # for line in self.arena["target_zone"]: - # self.ax.plot( - # [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red" - # ) - if len(self.pose_coords) > 0: - self.ax.quiver(self.pose_coords[0], self.pose_coords[1], self.pose_uv[0], self.pose_uv[1], color="blue") + self.ax.scatter(self.poses[:,0], self.poses[:,1], color="blue") async def send_command(self, command): #+ "\n" - why does adding this (which sounds right) cause the ble stack (on the robot or computer? ) not to work any more? diff --git a/ch-13/full-version/robot/code.py b/ch-13/full-version/robot/code.py index e92833e..5819813 100644 --- a/ch-13/full-version/robot/code.py +++ b/ch-13/full-version/robot/code.py @@ -28,8 +28,8 @@ class Simulation: # speed is proportional to distance from wall -> further we are from wall, faster we can go # turn is proportional to difference between left and right distance sensors. - self.forward_distance_pid = pid_controller.PIDController(0.1, 0.01, 0.01) - self.turn_pid = pid_controller.PIDController(0.1, 0.01, 0.01) + self.forward_distance_pid = pid_controller.PIDController(0.01, 0.001, 0.001) + self.turn_pid = pid_controller.PIDController(0.01, 0.001, 0.001) self.distance_aim = 100 def apply_sensor_model(self): @@ -88,7 +88,9 @@ class Simulation: weights[index] = 1 / (left_error + right_error) #normalise the weights + print("Weights sum before normalising:", np.sum(weights)) weights = weights / np.sum(weights) + print("Weights sum:", np.sum(weights)) return weights def resample(self, weights): @@ -107,7 +109,7 @@ class Simulation: cumulative_weights += weights[source_index] samples.append(source_index) # set poses to the resampled poses - self.poses = self.poses[samples] + self.poses = np.array([self.poses[n] for n in samples]) async def move_robot(self): """move forward, apply the motion model""" @@ -116,13 +118,14 @@ class Simulation: encoder_right = robot.right_encoder.read() # move forward - use distance sensor to determine how far to go - distance_error = self.distance_aim - min(self.left_distance, self.right_distance) + print("left_distance:", self.left_distance, "right_distance:", self.right_distance) + distance_error = min(self.left_distance, self.right_distance) - self.distance_aim forward_speed = self.forward_distance_pid.calculate(distance_error, self.time_step) turn_error = self.left_distance - self.right_distance turn_speed = self.turn_pid.calculate(turn_error, self.time_step) - - robot.set_left(forward_speed + turn_speed) - robot.set_right(forward_speed - turn_speed) + print("forward_speed:", forward_speed, "turn_speed:", turn_speed) + # robot.set_left(forward_speed + turn_speed) + # robot.set_right(forward_speed - turn_speed) await asyncio.sleep(self.time_step) # record sensor changes @@ -140,13 +143,14 @@ class Simulation: heading_standard_dev = 2 # degrees speed_standard_dev = 5 # mm - radians = np.radians(self.poses[2]) - heading_model = [get_gaussian_sample(0, heading_standard_dev) for _ in range(self.poses.shape[1])] - speed_model = [get_gaussian_sample(speed_in_mm, speed_standard_dev) for _ in range(self.poses.shape[1])] + radians = np.radians(self.poses[:,2]) + heading_model = np.array([get_gaussian_sample(0, heading_standard_dev) for _ in range(self.poses.shape[0])]) + speed_model = np.array([get_gaussian_sample(speed_in_mm, speed_standard_dev) for _ in range(self.poses.shape[0])]) + # print("Radians shape:", radians.shape, "heading_model shape:", len(heading_model), "speed_model shape:", len(speed_model), "poses shape:", self.poses.shape) self.poses[:,0] += speed_model * np.cos(radians) self.poses[:,1] += speed_model * np.sin(radians) - self.poses[:,2] += np.full(self.poses[2].shape, heading_change + heading_model) - self.poses[:,2] = np.vectorize(lambda n: n % 360)(self.poses[2]) + self.poses[:,2] += heading_change + heading_model + self.poses[:,2] = np.vectorize(lambda n: float(n % 360))(self.poses[:,2]) async def distance_sensor_updater(self): robot.left_distance.start_ranging() @@ -163,10 +167,14 @@ class Simulation: async def run(self): asyncio.create_task(self.distance_sensor_updater()) try: - for _ in range(15): + while True: + print("Applying sensor model") weights = self.apply_sensor_model() + print("Sensor model complete.\nResampling") self.resample(weights) + print("Resampling complete.\nMoving robot") await self.move_robot() + print("Robot move complete") finally: robot.stop() @@ -207,18 +215,22 @@ async def updater(simulation): } } ) - send_json( - { - "poses": simulation.poses.tolist(), - } - ) + # The big time delay is in sending the poses. + print("Sending poses", simulation.poses.shape[0]) + for n in range(0, simulation.poses.shape[0], 10): + print("Sending poses from ", n, "to", n+10, "of", simulation.poses.shape[0], "poses") + send_json({ + "poses": simulation.poses[n:n+10].tolist(), + "offset": n, + }) + await asyncio.sleep(0.01) await asyncio.sleep(0.5) async def command_handler(simulation): - update_task = asyncio.create_task(updater(simulation)) - simulation_task = asyncio.create_task(simulation.run()) print("Starting handler") + update_task = None + simulation_task = None while True: if robot.uart.in_waiting: print("Receiving data...") @@ -232,6 +244,11 @@ async def command_handler(simulation): "arena": arena.boundary_lines } ) + if not update_task: + update_task = asyncio.create_task(updater(simulation)) + elif request["command"] == "start": + simulation_task = asyncio.create_task(simulation.run()) + await asyncio.sleep(0.1)