From 642591ecc453149593c07519dad96570d3eee707 Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Mon, 10 Oct 2022 14:16:47 +0200 Subject: [PATCH] :sparkles: --- selflocalization/selflocalize.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py index e717be1..a0da519 100644 --- a/selflocalization/selflocalize.py +++ b/selflocalization/selflocalize.py @@ -160,12 +160,13 @@ def drive_towards_middle(est_pose, arlo): position = np.array([est_pose.x, est_pose.y]) theta = est_pose.theta + print(np.rad2deg(theta)) relative_pos = middle - position print("relative_pos", relative_pos) - angle = np.arctan(relative_pos[1]/relative_pos[0]) + angle = np.rad2deg(np.arctan(relative_pos[1]/relative_pos[0])) print(angle) - turn_angle = np.mod(np.rad2deg(angle - 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: