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

Put into a function.

Browse Source 
break down the
large function
This commit is contained in:
Danny Staple 2022-11-15 21:24:59 +00:00
parent 02c1f81756
commit 605b0afa9b
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ch-13/4-moving-poses-with-sensors-turning/robot/code.py
View File

@ -13,6 +13,13 @@ class Simulation:
for n in range(population_size):
self.poses[n] = random.uniform(0, arena.width), random.uniform(0, arena.height), random.uniform(0, 360)
def move_poses(self, speed_in_mm, heading_change):
for pose in self.poses:
pose[2] += heading_change
pose[2] = pose[2] % 360
pose[0] += speed_in_mm * np.cos(np.radians(pose[2]))
pose[1] += speed_in_mm * np.sin(np.radians(pose[2]))
async def run(self):
try:
for _ in range(5):
@ -27,11 +34,7 @@ class Simulation:
speed_in_mm = robot.ticks_to_m * ((left_movement + right_movement) / 2) * 1000
new_heading = robot.imu.euler[0]
heading_change = starting_heading - new_heading
for pose in self.poses:
pose[2] += heading_change
pose[2] = pose[2] % 360
pose[0] += speed_in_mm * np.cos(np.radians(pose[2]))
pose[1] += speed_in_mm * np.sin(np.radians(pose[2]))
self.move_poses(speed_in_mm, heading_change)
finally:
robot.stop()