From 4c89c36a78bddb10680c1311f74cf4a0ea9de2b5 Mon Sep 17 00:00:00 2001
From: NikolajDanger <nikolajgade@live.dk>
Date: Mon, 24 Oct 2022 14:27:38 +0200
Subject: [PATCH] :sparkles:

---
 rally.py | 46 ++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 38 insertions(+), 8 deletions(-)

diff --git a/rally.py b/rally.py
index 55f59aa..19d54ff 100644
--- a/rally.py
+++ b/rally.py
@@ -1,4 +1,4 @@
-from time import sleep
+import time
 
 import numpy as np
 import cv2
@@ -38,6 +38,29 @@ def find_aruco(image):
 
     return {ids[i][0]: box[0] for i, box in enumerate(corners)}
 
+def careful_forward(drive_time, arlo):
+    start = time.time()
+    end = start + drive_time
+    turning = None
+    while time.time() < end:
+        forward_dist = arlo.read_front_ping_sensor()
+        right_dist = arlo.read_right_ping_sensor()
+        left_dist = arlo.read_left_ping_sensor()
+        if forward_dist > 600 and all(x > 200 for x in [right_dist, left_dist]):
+            print("not blocked")
+            turning = None
+            arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
+        else:
+            print("blocked")
+            if turning == "R" or (forward_dist > 600 and right_dist > 200 and turning is None):
+                arlo.go_diff(POWER, POWER, 1, 0)
+                turning = "R"
+            else:
+                arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 0, 1)
+                turning = "L"
+
+        time.sleep(0.01)
+
 def main():
     landmark_order = LANDMARKS + [
         LANDMARKS[0]
@@ -49,12 +72,19 @@ def main():
         landmark = landmark_order.pop(0)
 
         while True:
-            arucos = find_aruco(noah.take_photo())
+            for _ in range(36):
+                arucos = find_aruco(noah.take_photo())
+                if landmark in arucos:
+                    break
+                noah.go_diff(POWER, POWER, 0, 1)
+                time.sleep((10 * TURN_T)/1000)
+                noah.stop()
+
             if landmark in arucos:
                 break
-            noah.go_diff(40, 40, 1, 0)
-            sleep(0.3)
-            noah.stop()
+
+            careful_forward(5, noah)
+
 
         position = cv2.aruco.estimatePoseSingleMarkers(
             np.array([arucos[landmark]]), 14.5, CAMERA_MATRIX, DIST_COEF
@@ -64,15 +94,15 @@ def main():
         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)
+            time.sleep((abs(angle) * TURN_T)/1000)
             noah.stop()
         else:
             noah.go_diff(POWER, POWER, 1, 0)
-            sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
+            time.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)
+        time.sleep((drive_distance * DRIVE_T)/1000)
         noah.stop()
 
 if __name__ == "__main__":