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