✨

drive_between_arucos.py

@@ -61,7 +61,6 @@ def find_arucos(arlo):
         sleep((20 * TURN_T)/1000)
         arlo.stop()
         for key, value in aruco_dict.items():
-            print(rot, value)
             aruco_dict[key] = np.dot(rot, value)
 
     return np.array(list(aruco_dict.values())[:2])
@@ -74,7 +73,7 @@ def main():
 
     position = [
         np.average(aruco_positions[:,0]),
-        np.average(aruco_positions[:,2])
+        np.average(aruco_positions[:,1])
     ]
 
     angle = np.rad2deg(np.arctan(position[0]/position[1]))