✨

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__":