diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py index 56e1a20..e717be1 100644 --- a/selflocalization/selflocalize.py +++ b/selflocalization/selflocalize.py @@ -165,7 +165,8 @@ def drive_towards_middle(est_pose, arlo): print("relative_pos", relative_pos) 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) - 180, 360) + print(turn_angle) drive_distance = np.sqrt(position[0]**2 + position[1]**2) if turn_angle < 180: arlo.go_diff(POWER, POWER, 0, 1)