✨

rally.py

@@ -71,6 +71,7 @@ def main():
     while landmark_order != []:
         landmark = landmark_order.pop(0)
 
+        while True:
             while True:
                 for _ in range(24):
                     arucos = find_aruco(noah.take_photo())
@@ -92,6 +93,9 @@ def main():
 
             angle = np.rad2deg(np.arctan(position[0]/position[2]))
             drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 10
+            if drive_distance < 30:
+                break
+
             if angle < 0:
                 noah.go_diff(POWER, POWER, 0, 1)
                 time.sleep((abs(angle) * TURN_T)/1000)
@@ -101,9 +105,7 @@ def main():
                 time.sleep((abs(angle) * TURN_T * CLOCKWISE_OFFSET)/1000)
                 noah.stop()
 
-        noah.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
-        time.sleep((drive_distance * DRIVE_T)/1000)
-        noah.stop()
+            careful_forward((drive_distance * DRIVE_T)/1000)
 
 if __name__ == "__main__":
     main()