diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py index a0da519..98223b5 100644 --- a/selflocalization/selflocalize.py +++ b/selflocalization/selflocalize.py @@ -166,7 +166,7 @@ def drive_towards_middle(est_pose, arlo): print("relative_pos", relative_pos) angle = np.rad2deg(np.arctan(relative_pos[1]/relative_pos[0])) 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) drive_distance = np.sqrt(position[0]**2 + position[1]**2) if turn_angle < 180: