From a7ffe3f58173ab4d18121ccc2de205e365856213 Mon Sep 17 00:00:00 2001
From: NikolajDanger <nikolajgade@live.dk>
Date: Wed, 26 Oct 2022 10:28:25 +0200
Subject: [PATCH] :skull: skeleton for rally

---
 rally2.py | 69 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 69 insertions(+)
 create mode 100644 rally2.py

diff --git a/rally2.py b/rally2.py
new file mode 100644
index 0000000..70fa554
--- /dev/null
+++ b/rally2.py
@@ -0,0 +1,69 @@
+import time
+import numpy as np
+import cv2
+
+import robot
+
+LANDMARKS = [7,11,10,6]
+
+LANDMARK_POSITIONS = {
+    7: [0,0],
+    11: [300,0],
+    10: [0,400],
+    6: [300,400]
+}
+
+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 look_around():
+    pass
+
+def turn_towards_landmark(est_pose, landmark):
+    pass
+
+def time_to_landmark(est_pose, landmark):
+    """Kør indenfor 1 meter"""
+    pass
+
+def drive_until_stopped(est_pose):
+    pass
+
+def inch_closer():
+    pass
+
+def main():
+    landmark_order = LANDMARKS + [
+        LANDMARKS[0]
+    ]
+
+    noah = robot.Robot()
+
+    for landmark in landmark_order:
+        while True:
+            est_pose = look_around()
+            est_pose = turn_towards_landmark(est_pose, landmark)
+            drive_time = time_to_landmark(est_pose, landmark)
+            if not drive_until_stopped(drive_time) == drive_time:
+                # drive around
+                continue
+            inch_closer()
+            break
+
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file