✨

drive_to_aruco.py

@@ -43,13 +43,19 @@ def main():
         if arucos != []:
             break
         arlo.go_diff(POWER, POWER, 1, 0)
-        sleep(TURN_T)
+        sleep(3 * TURN_T)
         arlo.stop()
 
     position = cv2.aruco.estimatePoseSingleMarkers(
         np.array([arucos[0][0]]), 14.5, CAMERA_MATRIX, DIST_COEF
-    )
+    )[1][0][0]
     print(position)
 
+    angle = np.rad2deg(np.arctan(position[0]/position[2]))
+    print(angle)
+
+    # drive_distance = np.sqrt(position[0]**2 + position[2]**2)
+
+
 if __name__ == "__main__":
     main()