✨

drive_between_arucos.py

@@ -43,7 +43,9 @@ def find_arucos(arlo):
         arucos = find_aruco(arlo.take_photo())
         if arucos != []:
             for aruco, id_ in arucos:
-                aruco_dict[id_] = aruco
+                if id_ in [1,6]:
+                    print(f"found box {id_}")
+                    aruco_dict[id_] = aruco
 
             if len(aruco_dict) >= 2:
                 break
@@ -51,7 +53,7 @@ def find_arucos(arlo):
         sleep(0.3)
         arlo.stop()
 
-    return list(aruco_dict.items())[:2]
+    return list(aruco_dict.values())[:2]
 
 def main():
     arlo = Robot()