ecki/Robotics-at-Home-with-Raspberry-Pi-Pico
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Make robust

Browse Source
This commit is contained in:
Danny Staple 2022-11-13 19:11:50 +00:00
parent 41ef282cd7
commit d1aa653e1b
1 changed files with 17 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -14,21 +14,21 @@ class Simulation:
self.poses[n] = random.uniform(0, arena.width), random.uniform(0, arena.height), random.uniform(0, 360) self.poses[n] = random.uniform(0, arena.width), random.uniform(0, arena.height), random.uniform(0, 360)
async def run(self): async def run(self):
for n in range(3): try:
encoder_left = robot.left_encoder.read() for _ in range(3):
encoder_right = robot.right_encoder.read() encoder_left = robot.left_encoder.read()
robot.set_left(0.5) encoder_right = robot.right_encoder.read()
robot.set_right(0.5) robot.set_left(0.5)
await asyncio.sleep(0.1) robot.set_right(0.5)
left_movement = robot.left_encoder.read() - encoder_left await asyncio.sleep(0.1)
right_movement = robot.right_encoder.read() - encoder_right left_movement = robot.left_encoder.read() - encoder_left
speed_in_mm = robot.ticks_to_m * ((left_movement + right_movement) / 2) * 1000 right_movement = robot.right_encoder.read() - encoder_right
for pose in self.poses: speed_in_mm = robot.ticks_to_m * ((left_movement + right_movement) / 2) * 1000
pose[0] += speed_in_mm * np.cos(np.radians(pose[2])) for pose in self.poses:
pose[1] += speed_in_mm * np.sin(np.radians(pose[2])) pose[0] += speed_in_mm * np.cos(np.radians(pose[2]))
# pose[2] += robot.ticks_to_degrees(left_movement - right_movement) / robot.wheel_separation_mm pose[1] += speed_in_mm * np.sin(np.radians(pose[2]))
# pose[2] = pose[2] % 360 finally:
robot.stop() robot.stop()
async def command_handler(simulation): async def command_handler(simulation):
simulation_task = None simulation_task = None
@ -55,11 +55,11 @@ async def command_handler(simulation):
robot.uart.write((json.dumps(response)+"\n").encode()) robot.uart.write((json.dumps(response)+"\n").encode())
elif request["command"] == "start": elif request["command"] == "start":
print("Starting simulation") print("Starting simulation")
if simulation_task is None: if simulation_task is None or simulation_task.done():
simulation_task = asyncio.create_task(simulation.run()) simulation_task = asyncio.create_task(simulation.run())
elif request["command"] == "stop": elif request["command"] == "stop":
robot.stop() robot.stop()
if simulation_task: if simulation_task and not simulation_task.done():
simulation_task.cancel() simulation_task.cancel()
simulation_task = None simulation_task = None
else: else: