From 7fb14f93455c0c1e43105687e6d2715845218a69 Mon Sep 17 00:00:00 2001 From: NikolajDanger Date: Wed, 28 Sep 2022 14:06:13 +0200 Subject: [PATCH] :sparkles: --- drive_between_arucos.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/drive_between_arucos.py b/drive_between_arucos.py index b5cbafd..761d23c 100644 --- a/drive_between_arucos.py +++ b/drive_between_arucos.py @@ -74,9 +74,10 @@ def main(): np.average(aruco_positions[:,0]), np.average(aruco_positions[:,1]) ] + print(position) angle = np.rad2deg(np.arctan(position[1]/position[0])) - drive_distance = np.sqrt(position[0]**2 + position[1]**2) + drive_distance = np.sqrt(position[0]**2 + position[1]**2) + 22.5 if angle < 0: arlo.go_diff(POWER, POWER, 0, 1) sleep((abs(angle) * TURN_T)/1000)