✨

rally.py

@@ -38,10 +38,33 @@ def find_aruco(image):
 
     return {ids[i][0]: box[0] for i, box in enumerate(corners)}
 
+
+def drunk_drive(drive_time, arlo, careful_range = 600):
+    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 > careful_range and all(x > 250 for x in [right_dist, left_dist]):
+            turning = None
+            arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
+        else:
+            if turning == "R" or (forward_dist > careful_range and right_dist > 250 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 careful_forward(drive_time, arlo, careful_range = 600):
     start = time.time()
-    if drive_time > 2:
-        drive_time = 2
+    if drive_time > 1:
+        drive_time = 1
         hit_something = True
     else:
         hit_something = False
@@ -92,7 +115,7 @@ def main():
                 if landmark in arucos:
                     break
 
-                careful_forward(3, noah)
+                drunk_drive(3, noah)
 
 
             position = cv2.aruco.estimatePoseSingleMarkers(