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-31 14:49:29 +01:00
parent 51b29110bc
commit 0621d02fff
1 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
rally2.py
View File

@ -254,7 +254,7 @@ def turn_90_degrees(noah, est_pose, particles):
current_position = np.array([est_pose.x, est_pose.y]) current_position = np.array([est_pose.x, est_pose.y])
relative_pos = middle - current_position relative_pos = middle - current_position
angle = np.arctan(relative_pos[1]/relative_pos[0]) angle = np.arctan(relative_pos[1]/relative_pos[0])
relative_angle = np.mod(angle - p.theta(), 2.0*np.pi) relative_angle = np.mod(angle - est_pose.theta(), 2.0*np.pi)
clockwise = relative_angle > 180 clockwise = relative_angle > 180