✨

rally.py

@@ -100,7 +100,7 @@ def main():
             )[1][0][0]
 
             angle = np.rad2deg(np.arctan(position[0]/position[2]))
-            drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 30
+            drive_distance = np.sqrt(position[0]**2 + position[2]**2) - 100
             if drive_distance < 30:
                 break
 
@@ -114,7 +114,16 @@ def main():
                 noah.stop()
 
             if not careful_forward((drive_distance * DRIVE_T)/1000, noah, 400):
-                break
+                arucos = find_aruco(noah.take_photo())
+                if landmark in arucos:
+                    position = cv2.aruco.estimatePoseSingleMarkers(
+                        np.array([arucos[landmark]]), 14.5, CAMERA_MATRIX, DIST_COEF
+                    )[1][0][0]
+                    noah.go_diff(POWER, POWER, 1, 1)
+                    time.sleep((90 * DRIVE_T)/1000)
+                    noah.stop()
+
+                    break
 
 if __name__ == "__main__":
     main()