From 2372c85148c45a75ba565ff9986327dec0cc016c Mon Sep 17 00:00:00 2001
From: NikolajDanger <nikolajgade@live.dk>
Date: Mon, 24 Oct 2022 14:03:29 +0200
Subject: [PATCH] :sparkles:

---
 rally/rally.py | 83 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 83 insertions(+)
 create mode 100644 rally/rally.py

diff --git a/rally/rally.py b/rally/rally.py
new file mode 100644
index 0000000..3d10fac
--- /dev/null
+++ b/rally/rally.py
@@ -0,0 +1,83 @@
+from time import sleep
+
+import numpy as np
+import cv2
+
+import robot
+
+LANDMARKS = [1,2,3,4] # SET ARUCO NUMBERS
+
+POWER = 70
+
+TURN_T = 7.9 # 1 degree
+DRIVE_T = 22 # 1 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 {ids[i]: box[0] for i, box in enumerate(corners)}
+
+def main():
+    landmark_order = [
+        LANDMARKS[0],
+        LANDMARKS[1],
+        LANDMARKS[2],
+        LANDMARKS[3],
+        LANDMARKS[0]
+    ]
+
+    noah = robot.Robot()
+
+    while landmark_order != []:
+        landmark = landmark_order.pop(0)
+
+        while True:
+            arucos = find_aruco(noah.take_photo())
+            if landmark in arucos:
+                break
+            noah.go_diff(40, 40, 1, 0)
+            sleep(0.3)
+            noah.stop()
+
+        position = cv2.aruco.estimatePoseSingleMarkers(
+            np.array([arucos[landmark][0]]), 14.5, CAMERA_MATRIX, DIST_COEF
+        )[1][0][0]
+
+        angle = np.rad2deg(np.arctan(position[0]/position[2]))
+        drive_distance = np.sqrt(position[0]**2 + position[2]**2)
+        if angle < 0:
+            noah.go_diff(POWER, POWER, 0, 1)
+            sleep((abs(angle) * TURN_T)/1000)
+            noah.stop()
+        else:
+            noah.go_diff(POWER, POWER, 1, 0)
+            sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
+            noah.stop()
+
+        noah.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
+        sleep((drive_distance * DRIVE_T)/1000)
+        noah.stop()
+
+if __name__ == "__main__":
+    main()