NikolajDanger/Roboteksperimentarium
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-10-10 14:10:22 +02:00
parent f2e3a7f56a
commit 0bad669ba5
1 changed files with 4 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
selflocalization/selflocalize.py
View File

@ -162,7 +162,9 @@ def drive_towards_middle(est_pose, arlo):
theta = est_pose.theta theta = est_pose.theta
relative_pos = middle - position relative_pos = middle - position
print("relative_pos", relative_pos)
angle = np.arctan(relative_pos[1]/relative_pos[0]) angle = np.arctan(relative_pos[1]/relative_pos[0])
print(angle)
turn_angle = np.mod(np.rad2deg(angle - theta), 360) turn_angle = np.mod(np.rad2deg(angle - theta), 360)
drive_distance = np.sqrt(position[0]**2 + position[1]**2) drive_distance = np.sqrt(position[0]**2 + position[1]**2)
if turn_angle < 180: if turn_angle < 180:
@ -175,7 +177,7 @@ def drive_towards_middle(est_pose, arlo):
arlo.stop() arlo.stop()
arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
sleep(0.5) sleep((drive_distance * DRIVE_T)/1000)
arlo.stop() arlo.stop()
# Main program # # Main program #
@ -263,6 +265,7 @@ try:
# Show world # Show world
cv2.imshow(WIN_World, world) cv2.imshow(WIN_World, world)
print(est_pose.x, est_pose.y, est_pose.theta)
drive_towards_middle(est_pose, arlo) drive_towards_middle(est_pose, arlo)