From b7ccb84c198bbb37b1a08d20077f705eb03f6322 Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Mon, 10 Oct 2022 14:14:01 +0200 Subject: [PATCH] :sparkles: --- selflocalization/selflocalize.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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)