From d1aa653e1b3007114f3c4703d5c81887eb6a5b8b Mon Sep 17 00:00:00 2001 From: Danny Staple Date: Sun, 13 Nov 2022 19:11:50 +0000 Subject: [PATCH] Make robust --- .../3-moving-poses-with-sensors/robot/code.py | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/ch-13/3-moving-poses-with-sensors/robot/code.py b/ch-13/3-moving-poses-with-sensors/robot/code.py index 3776064..ff8a2ac 100644 --- a/ch-13/3-moving-poses-with-sensors/robot/code.py +++ b/ch-13/3-moving-poses-with-sensors/robot/code.py @@ -14,21 +14,21 @@ class Simulation: 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() + try: + for _ 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])) + finally: + robot.stop() async def command_handler(simulation): simulation_task = None @@ -55,11 +55,11 @@ async def command_handler(simulation): robot.uart.write((json.dumps(response)+"\n").encode()) elif request["command"] == "start": print("Starting simulation") - if simulation_task is None: + 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: + if simulation_task and not simulation_task.done(): simulation_task.cancel() simulation_task = None else: