From c1a6ddeda4860e92fd5a5d45d4f6fa3be1ea3e63 Mon Sep 17 00:00:00 2001
From: NikolajDanger <nikolajgade@live.dk>
Date: Mon, 26 Sep 2022 14:28:27 +0200
Subject: [PATCH] :sparkles:

---
 drive_to_aruco.py | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/drive_to_aruco.py b/drive_to_aruco.py
index f4b9401..5afafec 100644
--- a/drive_to_aruco.py
+++ b/drive_to_aruco.py
@@ -43,7 +43,7 @@ def main():
         if arucos != []:
             break
         arlo.go_diff(POWER, POWER, 1, 0)
-        sleep((30 * TURN_T)/1000)
+        sleep((30 * TURN_T * CLOCKWISE_OFFSET)/1000)
         arlo.stop()
 
     position = cv2.aruco.estimatePoseSingleMarkers(
@@ -51,10 +51,19 @@ def main():
     )[1][0][0]
 
     angle = np.rad2deg(np.arctan(position[0]/position[2]))
-    print(angle)
-
     drive_distance = np.sqrt(position[0]**2 + position[2]**2)
-    print(drive_distance)
+    if angle < 0:
+        arlo.go_diff(POWER, POWER, 0, 1)
+        sleep((abs(angle) * TURN_T)/1000)
+        arlo.stop()
+    else:
+        arlo.go_diff(POWER, POWER, 1, 0)
+        sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
+        arlo.stop()
+
+    arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
+    sleep((drive_distance * DRIVE_T)/1000)
+    arlo.stop()
 
 
 if __name__ == "__main__":