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)
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: