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

---
 drive_to_aruco.py | 54 +++++++++++++++++++++++++++++++++++++++++++++++
 robot/arlo.py     | 18 ++++++++--------
 2 files changed, 63 insertions(+), 9 deletions(-)
 create mode 100644 drive_to_aruco.py

diff --git a/drive_to_aruco.py b/drive_to_aruco.py
new file mode 100644
index 0000000..0c8d8a4
--- /dev/null
+++ b/drive_to_aruco.py
@@ -0,0 +1,54 @@
+from time import sleep
+
+import cv2
+import numpy as np
+
+from robot import Robot
+
+POWER = 70
+
+TURN_T = 0.079 # 10 degrees
+DRIVE_T = 0.22 # 10 centimeter
+
+RIGHT_WHEEL_OFFSET = 4
+
+CLOCKWISE_OFFSET = 0.96
+
+FOCAL_LENGTH = 1691
+
+CAMERA_MATRIX = np.array(
+    [[FOCAL_LENGTH,0,512],[0,FOCAL_LENGTH,360],[0,0,1]],
+    dtype=np.float32
+)
+DIST_COEF = np.array([0,0,0,0,0], dtype=np.float32)
+
+def find_aruco(image):
+    aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
+    aruco_params = cv2.aruco.DetectorParameters_create()
+    corners, ids, _ = cv2.aruco.detectMarkers(
+        image,
+        aruco_dict,
+        parameters=aruco_params
+    )
+
+    if corners is None:
+        return []
+
+    return [(box[0], ids[i]) for i, box in enumerate(corners)]
+
+def main():
+    arlo = Robot()
+    while True:
+        arucos = find_aruco(Robot.take_photo())
+        if arucos is not []:
+            break
+        arlo.go_diff(POWER, POWER, 1, 0)
+        sleep(TURN_T)
+
+    position = cv2.aruco.estimatePoseSingleMarkers(
+        np.array([arucos[0][0]]), 14.5, CAMERA_MATRIX, DIST_COEF
+    )
+    print(position)
+
+if __name__ == "__main__":
+    main()
diff --git a/robot/arlo.py b/robot/arlo.py
index 4bf62d1..7138f9b 100644
--- a/robot/arlo.py
+++ b/robot/arlo.py
@@ -6,8 +6,8 @@ import numpy as np
 from .robot import Robot
 
 START_VALUES = [60, 60]
-THRESHOLD = 1.1
-SLEEP_TIME = 2
+THRESHOLD = 1.05
+TESTING_SLEEP_TIME = 2
 DEFAULT_CALIBRATION_CODE = "40.40-40.40-40.40_ff-ff-ff-ff"
 
 FOCAL_LENGTH = 1691
@@ -21,7 +21,7 @@ DIST_COEF = np.array([0,0,0,0,0], dtype=np.float32)
 def test_forward(arlo, l_power, r_power):
     arlo.reset_encoder_counts()
     arlo.go_diff(l_power, r_power, 1, 1)
-    sleep(SLEEP_TIME)
+    sleep(TESTING_SLEEP_TIME)
     arlo.stop()
     return (
         abs(int(arlo.read_left_wheel_encoder())),
@@ -31,7 +31,7 @@ def test_forward(arlo, l_power, r_power):
 def test_back(arlo, l_power, r_power):
     arlo.reset_encoder_counts()
     arlo.go_diff(l_power, r_power, 0, 0)
-    sleep(SLEEP_TIME)
+    sleep(TESTING_SLEEP_TIME)
     arlo.stop()
     return (
         abs(int(arlo.read_left_wheel_encoder())),
@@ -41,7 +41,7 @@ def test_back(arlo, l_power, r_power):
 def test_clockwise(arlo, l_power, r_power):
     arlo.reset_encoder_counts()
     arlo.go_diff(l_power, r_power, 1, 0)
-    sleep(SLEEP_TIME)
+    sleep(TESTING_SLEEP_TIME)
     arlo.stop()
     return (
         abs(int(arlo.read_left_wheel_encoder())),
@@ -51,7 +51,7 @@ def test_clockwise(arlo, l_power, r_power):
 def test_anticlockwise(arlo, l_power, r_power):
     arlo.reset_encoder_counts()
     arlo.go_diff(l_power, r_power, 0, 1)
-    sleep(SLEEP_TIME)
+    sleep(TESTING_SLEEP_TIME)
     arlo.stop()
     return (
         abs(int(arlo.read_left_wheel_encoder())),
@@ -181,15 +181,15 @@ class Arlo():
                 else:
                     values[i][1] += 1
 
-                cps[i] = wheels[0]/SLEEP_TIME
+                cps[i] = wheels[0]/TESTING_SLEEP_TIME
 
         time = [0 for _ in range(4)]
 
         cpc = 144/(3.14159*15) # wheel counts per cm
 
-        # milliseconds per 10cm forward
+        # milliseconds per 1cm forward
         time[0] = int((1000 * cpc)/cps[0])
-        # milliseconds per 10cm backwards
+        # milliseconds per 1cm backwards
         time[1] = int((1000 * cpc)/cps[1])
 
         cpr = 3.1415*38 # 1 rotation in cm