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

---
 drive_to_aruco.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/drive_to_aruco.py b/drive_to_aruco.py
index 0c72009..f4b9401 100644
--- a/drive_to_aruco.py
+++ b/drive_to_aruco.py
@@ -7,8 +7,8 @@ from robot import Robot
 
 POWER = 70
 
-TURN_T = 0.079 # 10 degrees
-DRIVE_T = 0.22 # 10 centimeter
+TURN_T = 7.9 # 1 degree
+DRIVE_T = 22 # 1 centimeter
 
 RIGHT_WHEEL_OFFSET = 4
 
@@ -43,18 +43,18 @@ def main():
         if arucos != []:
             break
         arlo.go_diff(POWER, POWER, 1, 0)
-        sleep(3 * TURN_T)
+        sleep((30 * TURN_T)/1000)
         arlo.stop()
 
     position = cv2.aruco.estimatePoseSingleMarkers(
         np.array([arucos[0][0]]), 14.5, CAMERA_MATRIX, DIST_COEF
     )[1][0][0]
-    print(position)
 
     angle = np.rad2deg(np.arctan(position[0]/position[2]))
     print(angle)
 
-    # drive_distance = np.sqrt(position[0]**2 + position[2]**2)
+    drive_distance = np.sqrt(position[0]**2 + position[2]**2)
+    print(drive_distance)
 
 
 if __name__ == "__main__":