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:23:26 +02:00
parent 642591ecc4
commit 8a2156a5d9
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
selflocalization/selflocalize.py
View File

@ -166,7 +166,7 @@ def drive_towards_middle(est_pose, arlo):
print("relative_pos", relative_pos) print("relative_pos", relative_pos)
angle = np.rad2deg(np.arctan(relative_pos[1]/relative_pos[0])) angle = np.rad2deg(np.arctan(relative_pos[1]/relative_pos[0]))
print(angle) print(angle)
turn_angle = np.mod(angle - np.rad2deg(theta) - 180, 360) turn_angle = np.mod(angle - (np.rad2deg(theta) - 180), 360)
print(turn_angle) print(turn_angle)
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: