Autonomous driving Aruco task

autonomy/lib/constants.dart

@@ -13,7 +13,7 @@ class Constants {
   static const gpsError = 0.00003;
 
   /// The maximum error or "tolerance" for reaching the end goal
-  static const double maxErrorMeters = 1;
+  static const double maxErrorMeters = 1.5;
 
   /// The closest distance the pathfinding algorithm will allow
   /// the rover to go near an obstacle
@@ -30,11 +30,11 @@ class Constants {
   static const double replanErrorMeters = 3;
 
   /// The IMU angle tolerance for a turn during autonomy
-  static const double turnEpsilon = 3;
+  static const double turnEpsilon = 1;
 
   /// The IMU angle tolerance when turning to re-correct to the
   /// proper orientation before driving forward
-  static const double driveRealignmentEpsilon = 5;
+  static const double driveRealignmentEpsilon = 3;
 
   /// The maximum time to spend waiting for the drive to reach a desired GPS coordinate.
   ///
@@ -46,7 +46,7 @@ class Constants {
   );
 
   /// The maximum time to spend searching for an aruco tag
-  static const Duration arucoSearchTimeout = Duration(seconds: 20);
+  static const Duration arucoSearchTimeout = Duration(seconds: 30);
 
   /// The camera that should be used to detect Aruco tags
   static const CameraName arucoDetectionCamera = CameraName.ROVER_FRONT;

autonomy/lib/src/drive/drive_interface.dart

@@ -76,6 +76,9 @@ abstract class DriveInterface extends Service {
   StateInterface turnStateState(AutonomyAStarState state) =>
       faceOrientationState(state.orientation.orientation);
 
+  /// State to spin towards the specified aruco tag
+  StateInterface spinForArucoState(int arucoId, {CameraName? desiredCamera});
+
   /// Stop the rover
   Future<bool> stop();
 
@@ -131,6 +134,8 @@ abstract class DriveInterface extends Service {
       blink: blink ? BoolState.YES : BoolState.NO,
     );
     sendCommand(command);
+    sendCommand(command);
+    sendCommand(command);
   }
 
   /// Spin to face an Aruco tag, returns whether or not it was able to face the tag

autonomy/lib/src/drive/rover_drive.dart

@@ -142,4 +142,16 @@ class RoverDrive extends DriveInterface {
       return simDrive.faceOrientationState(orientation);
     }
   }
+
+  @override
+  StateInterface spinForArucoState(int arucoId, {CameraName? desiredCamera}) {
+    if (useImu) {
+      return sensorDrive.spinForArucoState(
+        arucoId,
+        desiredCamera: desiredCamera,
+      );
+    } else {
+      return simDrive.spinForArucoState(arucoId, desiredCamera: desiredCamera);
+    }
+  }
 }

autonomy/lib/src/drive/sensor_drive.dart

@@ -1,4 +1,5 @@
 import "package:autonomy/autonomy.dart";
+import "package:autonomy/src/state_machine/rover_states/spin_for_aruco.dart";
 
 import "drive_commands.dart";
 
@@ -42,6 +43,16 @@ class SensorDrive extends DriveInterface with RoverDriveCommands {
         drive: this,
       );
 
+  @override
+  StateInterface spinForArucoState(int arucoId, {CameraName? desiredCamera}) =>
+      SpinForAruco(
+        controller,
+        drive: this,
+        arucoId: arucoId,
+        desiredCamera: desiredCamera,
+        collection: collection,
+      );
+
   @override
   Future<bool> stop() async {
     stopMotors();

autonomy/lib/src/drive/sim_drive.dart

@@ -54,6 +54,13 @@ class DriveSimulator extends DriveInterface {
         goalOrientation: orientation,
       );
 
+  @override
+  StateInterface spinForArucoState(int arucoId, {CameraName? desiredCamera}) =>
+      FunctionalState(
+        controller,
+        onEnter: (controller) => controller.popState(),
+      );
+
   @override
   Future<bool> init() async => true;
 

autonomy/lib/src/drive/timed_drive.dart

@@ -52,6 +52,10 @@ class TimedDrive extends DriveInterface with RoverDriveCommands {
     );
   }
 
+  @override
+  StateInterface spinForArucoState(int arucoId, {CameraName? desiredCamera}) =>
+      throw UnsupportedError("Cannot spin for aruco using TimedDrive");
+
   @override
   StateInterface driveForwardState(GpsCoordinates coordinates) =>
       _TimedOperationState(