From 4d06d6af118976cf7f4a2d4a13e681b1673ade77 Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Tue, 27 Jan 2026 16:25:00 -0500 Subject: [PATCH 1/6] Initial implementation of aruco task with state machine --- autonomy/lib/src/drive/drive_interface.dart | 3 + autonomy/lib/src/drive/rover_drive.dart | 12 + autonomy/lib/src/drive/sensor_drive.dart | 10 + autonomy/lib/src/drive/sim_drive.dart | 7 + autonomy/lib/src/drive/timed_drive.dart | 4 + .../src/orchestrator/rover_orchestrator.dart | 377 +++++++++++------- .../rover_states/deferred_state.dart | 12 + .../rover_states/spin_for_aruco.dart | 62 +++ 8 files changed, 350 insertions(+), 137 deletions(-) create mode 100644 autonomy/lib/src/state_machine/rover_states/deferred_state.dart create mode 100644 autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart diff --git a/autonomy/lib/src/drive/drive_interface.dart b/autonomy/lib/src/drive/drive_interface.dart index c87ca3ba..98995c93 100644 --- a/autonomy/lib/src/drive/drive_interface.dart +++ b/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 stop(); diff --git a/autonomy/lib/src/drive/rover_drive.dart b/autonomy/lib/src/drive/rover_drive.dart index 91064713..48048775 100644 --- a/autonomy/lib/src/drive/rover_drive.dart +++ b/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); + } + } } diff --git a/autonomy/lib/src/drive/sensor_drive.dart b/autonomy/lib/src/drive/sensor_drive.dart index 3b5302ff..15882717 100644 --- a/autonomy/lib/src/drive/sensor_drive.dart +++ b/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,15 @@ class SensorDrive extends DriveInterface with RoverDriveCommands { drive: this, ); + @override + StateInterface spinForArucoState(int arucoId, {CameraName? desiredCamera}) => + SpinForAruco( + controller, + drive: this, + arucoId: arucoId, + collection: collection, + ); + @override Future stop() async { stopMotors(); diff --git a/autonomy/lib/src/drive/sim_drive.dart b/autonomy/lib/src/drive/sim_drive.dart index 3626b495..2d542f84 100644 --- a/autonomy/lib/src/drive/sim_drive.dart +++ b/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 init() async => true; diff --git a/autonomy/lib/src/drive/timed_drive.dart b/autonomy/lib/src/drive/timed_drive.dart index 131ff8ab..4c5546cb 100644 --- a/autonomy/lib/src/drive/timed_drive.dart +++ b/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( diff --git a/autonomy/lib/src/orchestrator/rover_orchestrator.dart b/autonomy/lib/src/orchestrator/rover_orchestrator.dart index 8e05835d..95381692 100644 --- a/autonomy/lib/src/orchestrator/rover_orchestrator.dart +++ b/autonomy/lib/src/orchestrator/rover_orchestrator.dart @@ -2,6 +2,7 @@ import "dart:math"; import "package:autonomy/constants.dart"; import "package:autonomy/interfaces.dart"; +import "package:autonomy/src/state_machine/rover_states/deferred_state.dart"; import "dart:async"; import "package:coordinate_converter/coordinate_converter.dart"; @@ -273,155 +274,257 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { // Go to GPS coordinates collection.logger.info("Got ArUco Task"); - DetectedObject? detectedAruco; + var driveToDestinationFailed = false; + var arucoSearchFailed = false; - if (command.destination != GpsCoordinates(latitude: 0, longitude: 0)) { - if (!await calculateAndFollowPath( - command.destination, - abortOnError: false, - )) { - collection.logger.error( - "Failed to follow path towards initial destination", - ); - currentState = AutonomyState.NO_SOLUTION; - currentCommand = null; - return; - } - } - - currentState = AutonomyState.SEARCHING; - collection.logger.info("Searching for ArUco tag"); - final didSeeAruco = await collection.drive.spinForAruco( - command.arucoId, - desiredCamera: Constants.arucoDetectionCamera, - ); - detectedAruco = collection.video.getArucoDetection( - command.arucoId, - desiredCamera: Constants.arucoDetectionCamera, - ); + final arucoTaskSequence = SequenceState( + controller, + steps: [ + if (command.destination != + GpsCoordinates(latitude: 0, longitude: 0)) ...[ + PathingState( + controller, + collection: collection, + orchestrator: this, + destination: command.destination, + ), + FunctionalState( + controller, + onEnter: (controller) { + if (!collection.gps.isNear( + command.destination, + Constants.maxErrorMeters, + )) { + // handle error + driveToDestinationFailed = true; + } else { + // starting search + } + controller.popState(); + }, + ), + ], + // Find the aruco tag + FunctionalState( + controller, + onEnter: (controller) { + currentState = AutonomyState.SEARCHING; + controller.popState(); + }, + ), + collection.drive.spinForArucoState( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ), + // Check if tag was found and face it + DeferredState(controller, (controller) { + final detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + if (detectedAruco == null) { + currentState = AutonomyState.NO_SOLUTION; + return FunctionalState( + controller, + onEnter: (controller) { + arucoSearchFailed = true; + controller.popState(); + }, + ); + } + collection.logger.info("Found aruco"); + currentState = AutonomyState.APPROACHING; + return collection.drive.faceOrientationState( + Orientation(z: collection.imu.heading - detectedAruco.yaw), + ); + }), + // Wait 3 seconds for a detection + TimeoutDecorator( + timeout: const Duration(seconds: 3), + onTimeout: (controller) => controller.popState(), + child: FunctionalState( + controller, + onUpdate: (controller) { + if (collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ) != + null) { + controller.popState(); + } + }, + ), + ), + DeferredState(controller, (controller) { + final detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + if (detectedAruco == null || !detectedAruco.hasBestPnpResult()) { + currentState = AutonomyState.NO_SOLUTION; + return FunctionalState( + controller, + onEnter: (controller) { + collection.logger.error( + "Could not find desired Aruco tag after rotating towards it", + ); + currentState = AutonomyState.NO_SOLUTION; + arucoSearchFailed = true; + }, + ); + } + collection.logger.debug( + "Planning path to Aruco ID ${command.arucoId}", + body: "Detection: ${detectedAruco.toProto3Json()}", + ); + // In theory we could just find the relative position with the translation x and z, + // however if the tag's rotation relative to itself is off (which can be common + // when facing it head on), then it will be extremely innacurate. Since the SolvePnP's + // distance is always extremely accurate, it is more reliable to use the distance + // hypotenuse to the camera combined with trig of the tag's angle relative to the camera. + final cameraToTag = detectedAruco.bestPnpResult.cameraToTarget; + final distanceToTag = + sqrt( + pow(cameraToTag.translation.z, 2) + + pow(cameraToTag.translation.x, 2), + ) - + 1; // don't drive *into* the tag + + if (distanceToTag < 1) { + // well that was easy + return FunctionalState( + controller, + onEnter: (controller) { + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + currentState = AutonomyState.AT_DESTINATION; + controller.popState(); + }, + ); + } - if (!didSeeAruco || detectedAruco == null) { - collection.logger.error("Could not find desired Aruco tag"); - currentState = AutonomyState.NO_SOLUTION; - currentCommand = null; - return; - } + final relativeX = + -distanceToTag * + sin((collection.imu.heading - detectedAruco.yaw) * pi / 180); + final relativeY = + distanceToTag * + cos((collection.imu.heading - detectedAruco.yaw) * pi / 180); - collection.logger.info("Found aruco"); - currentState = AutonomyState.APPROACHING; - final arucoOrientation = Orientation( - z: collection.imu.heading - detectedAruco.yaw, - ); - await collection.drive.faceOrientation(arucoOrientation); - detectedAruco = await collection.video.waitForAruco( - command.arucoId, - desiredCamera: Constants.arucoDetectionCamera, - timeout: const Duration(seconds: 3), - ); + final destinationCoordinates = + (collection.gps.coordinates.toUTM() + + UTMCoordinates(y: relativeY, x: relativeX, zoneNumber: 1)) + .toGps(); - if (detectedAruco == null || !detectedAruco.hasBestPnpResult()) { - // TODO: handle this condition properly - collection.logger.error( - "Could not find desired Aruco tag after rotating towards it", - ); - currentState = AutonomyState.NO_SOLUTION; - currentCommand = null; - return; - } + return PathingState( + controller, + collection: collection, + orchestrator: this, + destination: destinationCoordinates, + ); + }), - collection.logger.debug( - "Planning path to Aruco ID ${command.arucoId}", - body: "Detection: ${detectedAruco.toProto3Json()}", + // Re-spin towards tag + DeferredState(controller, (controller) { + final detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + collection.logger.info("Arrived at estimated Aruco position"); + if (detectedAruco == null) { + collection.logger.info("Re-spinning to find Aruco"); + // Re-find aruco, and point towards it + return SequenceState( + controller, + steps: [ + collection.drive.spinForArucoState( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ), + DeferredState(controller, (controller) { + final detectedAruco = collection.video.getArucoDetection( + command.arucoId, + desiredCamera: Constants.arucoDetectionCamera, + ); + if (detectedAruco != null) { + collection.logger.info("Rotating towards Aruco"); + return collection.drive.faceOrientationState( + Orientation( + z: collection.imu.heading - detectedAruco.yaw, + ), + ); + } else { + return FunctionalState( + controller, + onEnter: (controller) { + collection.logger.warning( + "Could not find Aruco after following path", + ); + controller.popState(); + }, + ); + } + }), + ], + ); + } + return FunctionalState( + controller, + onEnter: (controller) => controller.popState(), + ); + }), + FunctionalState( + controller, + onEnter: (controller) { + collection.logger.info( + "Successfully reached within ${Constants.maxErrorMeters} meters of the Aruco tag", + ); + currentState = AutonomyState.AT_DESTINATION; + collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); + controller.popState(); + // TODO: end command execution, we are done + }, + ), + ], ); - // In theory we could just find the relative position with the translation x and z, - // however if the tag's rotation relative to itself is off (which can be common - // when facing it head on), then it will be extremely innacurate. Since the SolvePnP's - // distance is always extremely accurate, it is more reliable to use the distance - // hypotenuse to the camera combined with trig of the tag's angle relative to the camera. - final cameraToTag = detectedAruco.bestPnpResult.cameraToTarget; - final distanceToTag = - sqrt( - pow(cameraToTag.translation.z, 2) + pow(cameraToTag.translation.x, 2), - ) - - 1; // don't drive *into* the tag - - if (distanceToTag < 1) { - // well that was easy - collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); - currentState = AutonomyState.AT_DESTINATION; - currentCommand = null; - return; - } + controller.pushState(arucoTaskSequence); - final relativeX = - -distanceToTag * - sin((collection.imu.heading - detectedAruco.yaw) * pi / 180); - final relativeY = - distanceToTag * - cos((collection.imu.heading - detectedAruco.yaw) * pi / 180); - - final destinationCoordinates = - (collection.gps.coordinates.toUTM() + - UTMCoordinates(y: relativeY, x: relativeX, zoneNumber: 1)) - .toGps(); - - if (!await calculateAndFollowPath( - destinationCoordinates, - abortOnError: false, - alternateEndCondition: () { - detectedAruco = collection.video.getArucoDetection( - command.arucoId, - desiredCamera: Constants.arucoDetectionCamera, - ); - if (detectedAruco == null) { - return false; - } - final cameraToTag = detectedAruco!.bestPnpResult.cameraToTarget; - final distanceToTag = sqrt( - pow(cameraToTag.translation.z, 2) + pow(cameraToTag.translation.x, 2), + executionTimer = PeriodicTimer(const Duration(milliseconds: 10), ( + timer, + ) async { + if (currentCommand == null) { + collection.logger.warning( + "Execution timer running while command is null", + body: "Canceling timer", ); - return distanceToTag < 1; - }, - )) { - collection.logger.error("Could not spin towards ArUco tag"); - currentState = AutonomyState.NO_SOLUTION; - currentCommand = null; - return; - } - collection.logger.info("Arrived at estimated Aruco position"); - detectedAruco = collection.video.getArucoDetection( - command.arucoId, - desiredCamera: Constants.arucoDetectionCamera, - ); - if (detectedAruco == null) { - collection.logger.info("Re-spinning to find Aruco"); - await collection.drive.spinForAruco( - command.arucoId, - desiredCamera: Constants.arucoDetectionCamera, - ); - } + onCommandEnd(); + timer.cancel(); + return; + } - detectedAruco = collection.video.getArucoDetection( - command.arucoId, - desiredCamera: Constants.arucoDetectionCamera, - ); - if (detectedAruco != null) { - collection.logger.info("Rotating towards Aruco"); - await collection.drive.faceOrientation( - Orientation(z: collection.imu.heading - detectedAruco!.yaw), - ); - } else { - collection.logger.warning("Could not find Aruco after following path"); - } + if (driveToDestinationFailed) { + collection.logger.error("Failed to drive to initial destination"); + onCommandEnd(); + timer.cancel(); + return; + } - collection.logger.info( - "Successfully reached within ${Constants.maxErrorMeters} meters of the Aruco tag", - ); - collection.drive.setLedStrip(ProtoColor.GREEN, blink: true); - currentState = AutonomyState.AT_DESTINATION; + if (arucoSearchFailed) { + collection.logger.error("Failed to find an Aruco tag"); + currentState = AutonomyState.NO_SOLUTION; + onCommandEnd(); + timer.cancel(); + return; + } - currentCommand = null; + if (!controller.hasState()) { + currentState = AutonomyState.NO_SOLUTION; + onCommandEnd(); + timer.cancel(); + return; + } + + controller.update(); + }); } @override diff --git a/autonomy/lib/src/state_machine/rover_states/deferred_state.dart b/autonomy/lib/src/state_machine/rover_states/deferred_state.dart new file mode 100644 index 00000000..dc51e42b --- /dev/null +++ b/autonomy/lib/src/state_machine/rover_states/deferred_state.dart @@ -0,0 +1,12 @@ +import "package:autonomy/autonomy.dart"; + +class DeferredState extends RoverState { + final StateInterface Function(StateController controller) supplier; + + DeferredState(super.controller, this.supplier); + + @override + void enter() { + controller.transitionTo(supplier(controller)); + } +} diff --git a/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart b/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart new file mode 100644 index 00000000..a4436e5e --- /dev/null +++ b/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart @@ -0,0 +1,62 @@ +import "package:autonomy/autonomy.dart"; +import "package:autonomy/src/drive/drive_commands.dart"; +import "package:autonomy/src/drive/drive_config.dart"; + +class SpinForAruco extends RoverState { + final AutonomyInterface collection; + final RoverDriveCommands drive; + + DriveConfig get config => collection.drive.config; + + final int arucoId; + final CameraName? desiredCamera; + + double _startOrientation = 0; + bool _rotated180 = false; + + SpinForAruco( + super.controller, { + required this.drive, + required this.arucoId, + required this.collection, + this.desiredCamera, + }); + + @override + void enter() { + _startOrientation = collection.imu.heading; + _rotated180 = false; + } + + @override + void update() { + drive.setThrottle(config.turnThrottle); + + if (collection.video.getArucoDetection( + arucoId, + desiredCamera: desiredCamera, + ) != + null) { + controller.popState(); + return; + } + + drive.spinLeft(); + + if ((collection.imu.heading - _startOrientation).clampHalfAngle().abs() > + 175) { + _rotated180 = true; + } + + if (_rotated180 && + collection.imu.isNear(Orientation(z: _startOrientation))) { + controller.popState(); + return; + } + } + + @override + void exit() { + drive.stop(); + } +} From ce6b4a4742f29e8efea7fdc3f6c9b78a115829ee Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Wed, 11 Feb 2026 13:33:02 -0500 Subject: [PATCH 2/6] Fixes from testing: 1. Raise tolerance 2. Send multiple LED strip commands --- autonomy/lib/constants.dart | 8 ++++---- autonomy/lib/src/drive/drive_interface.dart | 2 ++ 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/autonomy/lib/constants.dart b/autonomy/lib/constants.dart index b6e121fc..6383d9cb 100644 --- a/autonomy/lib/constants.dart +++ b/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; diff --git a/autonomy/lib/src/drive/drive_interface.dart b/autonomy/lib/src/drive/drive_interface.dart index 98995c93..2c7acea0 100644 --- a/autonomy/lib/src/drive/drive_interface.dart +++ b/autonomy/lib/src/drive/drive_interface.dart @@ -134,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 From 462ac61b879694ed264894974b8aed689a0c3136 Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Wed, 11 Feb 2026 13:54:57 -0500 Subject: [PATCH 3/6] Fix tests --- autonomy/test/network_test.dart | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/autonomy/test/network_test.dart b/autonomy/test/network_test.dart index 78912f24..fe71bf5c 100644 --- a/autonomy/test/network_test.dart +++ b/autonomy/test/network_test.dart @@ -139,8 +139,8 @@ void main() => group("[Network]", tags: ["network"], () { expect(subsystems.throttle, 0); expect(subsystems.left, 0); expect(subsystems.right, 0); - expect(simulator.gps.isNear(origin), isTrue); - expect(simulator.gps.isNear(oneMeter), isFalse); + expect(simulator.gps.isNear(origin, 1), isTrue); + expect(simulator.gps.isNear(oneMeter, 1), isFalse); expect(subsystems.throttleFlag, isFalse); final forwardFuture = Future.delayed( @@ -158,8 +158,8 @@ void main() => group("[Network]", tags: ["network"], () { expect(subsystems.throttle, isNot(0)); expect(subsystems.left, isNot(0)); expect(subsystems.right, isNot(0)); - expect(simulator.gps.isNear(origin), isTrue); - expect(simulator.gps.isNear(oneMeter), isFalse); + expect(simulator.gps.isNear(origin, 1), isTrue); + expect(simulator.gps.isNear(oneMeter, 1), isFalse); simulator.orchestrator.controller.update(); simulator.orchestrator.controller.update(); From 889f02787666898fd30ee3b7f399b743c2e03c0a Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Tue, 17 Feb 2026 11:28:56 -0500 Subject: [PATCH 4/6] Check for completion in navigation state --- autonomy/lib/src/state_machine/rover_states/navigation.dart | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/autonomy/lib/src/state_machine/rover_states/navigation.dart b/autonomy/lib/src/state_machine/rover_states/navigation.dart index c20f81dc..0b3fab8e 100644 --- a/autonomy/lib/src/state_machine/rover_states/navigation.dart +++ b/autonomy/lib/src/state_machine/rover_states/navigation.dart @@ -132,6 +132,12 @@ class NavigationState extends RoverState { @override void update() { + if (collection.gps.isNear(destination, Constants.maxErrorMeters)) { + collection.drive.stop(); + controller.popState(); + return; + } + if (currentPathState == null) { controller.popState(); return; From 5d432068103a77e74e86ac2cd3b5b92f7f37ae17 Mon Sep 17 00:00:00 2001 From: Binghamton Rover Date: Tue, 3 Feb 2026 20:38:25 -0500 Subject: [PATCH 5/6] Testing on rover --- autonomy/lib/src/drive/sensor_drive.dart | 1 + .../src/orchestrator/rover_orchestrator.dart | 4 ++- .../rover_states/navigation.dart | 31 ++++++++++++++++--- .../state_machine/rover_states/pathing.dart | 5 +++ .../rover_states/spin_for_aruco.dart | 14 ++++----- 5 files changed, 42 insertions(+), 13 deletions(-) diff --git a/autonomy/lib/src/drive/sensor_drive.dart b/autonomy/lib/src/drive/sensor_drive.dart index 15882717..c79a8f05 100644 --- a/autonomy/lib/src/drive/sensor_drive.dart +++ b/autonomy/lib/src/drive/sensor_drive.dart @@ -49,6 +49,7 @@ class SensorDrive extends DriveInterface with RoverDriveCommands { controller, drive: this, arucoId: arucoId, + desiredCamera: desiredCamera, collection: collection, ); diff --git a/autonomy/lib/src/orchestrator/rover_orchestrator.dart b/autonomy/lib/src/orchestrator/rover_orchestrator.dart index 95381692..11a432ed 100644 --- a/autonomy/lib/src/orchestrator/rover_orchestrator.dart +++ b/autonomy/lib/src/orchestrator/rover_orchestrator.dart @@ -287,13 +287,14 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { collection: collection, orchestrator: this, destination: command.destination, + tolerance: 3, ), FunctionalState( controller, onEnter: (controller) { if (!collection.gps.isNear( command.destination, - Constants.maxErrorMeters, + 3.25, )) { // handle error driveToDestinationFailed = true; @@ -419,6 +420,7 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { collection: collection, orchestrator: this, destination: destinationCoordinates, + tolerance: 2, ); }), diff --git a/autonomy/lib/src/state_machine/rover_states/navigation.dart b/autonomy/lib/src/state_machine/rover_states/navigation.dart index 0b3fab8e..101d54b9 100644 --- a/autonomy/lib/src/state_machine/rover_states/navigation.dart +++ b/autonomy/lib/src/state_machine/rover_states/navigation.dart @@ -1,3 +1,4 @@ +import "dart:async"; import "dart:math"; import "package:autonomy/constants.dart"; @@ -23,6 +24,9 @@ class NavigationState extends RoverState { /// The final destination to navigate to final GpsCoordinates destination; + // The tolerance of how to be to the destination + final double tolerance; + /// Whether or not the state has performed pre-step correction bool hasCorrected = false; @@ -32,6 +36,8 @@ class NavigationState extends RoverState { /// The index of the waypoint being followed int waypointIndex = 0; + Timer? _doneCheckTimer; + /// The current step of the path being followed AutonomyAStarState? get currentPathState { if (orchestrator.currentPath == null || @@ -47,6 +53,7 @@ class NavigationState extends RoverState { required this.collection, required this.orchestrator, required this.destination, + this.tolerance = Constants.maxErrorMeters, }); @override @@ -56,6 +63,11 @@ class NavigationState extends RoverState { hasFollowed = false; orchestrator.currentState = AutonomyState.DRIVING; + + _doneCheckTimer = Timer.periodic( + const Duration(milliseconds: 20), + _checkIfDone, + ); } /// Checks if the rover is oriented properly before driving the [state] @@ -110,6 +122,7 @@ class NavigationState extends RoverState { collection: collection, orchestrator: orchestrator, destination: destination, + tolerance: tolerance, ), ); return true; @@ -130,14 +143,18 @@ class NavigationState extends RoverState { } } - @override - void update() { - if (collection.gps.isNear(destination, Constants.maxErrorMeters)) { - collection.drive.stop(); + void _checkIfDone([_]) { + if (collection.gps.isNear(destination, tolerance)) { + controller.popUntil(); controller.popState(); - return; + collection.drive.stop(); + _doneCheckTimer?.cancel(); + _doneCheckTimer = null; } + } + @override + void update() { if (currentPathState == null) { controller.popState(); return; @@ -164,6 +181,7 @@ class NavigationState extends RoverState { collection: collection, orchestrator: orchestrator, destination: destination, + tolerance: tolerance, ), ); return; @@ -178,6 +196,7 @@ class NavigationState extends RoverState { collection: collection, orchestrator: orchestrator, destination: destination, + tolerance: tolerance, ), ); return; @@ -193,5 +212,7 @@ class NavigationState extends RoverState { if (currentPathState != null) { orchestrator.traversed.add(currentPathState!.position); } + _doneCheckTimer?.cancel(); + _doneCheckTimer = null; } } diff --git a/autonomy/lib/src/state_machine/rover_states/pathing.dart b/autonomy/lib/src/state_machine/rover_states/pathing.dart index 81166038..ce50e430 100644 --- a/autonomy/lib/src/state_machine/rover_states/pathing.dart +++ b/autonomy/lib/src/state_machine/rover_states/pathing.dart @@ -15,12 +15,16 @@ class PathingState extends RoverState { /// The destination to plan a path to final GpsCoordinates destination; + // The tolerance of how to be to the destination + final double tolerance; + /// Default constructor for [PathingState] PathingState( super.controller, { required this.collection, required this.orchestrator, required this.destination, + this.tolerance = Constants.maxErrorMeters, }); @override @@ -58,6 +62,7 @@ class PathingState extends RoverState { collection: collection, orchestrator: orchestrator, destination: destination, + tolerance: tolerance, ), ); } diff --git a/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart b/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart index a4436e5e..7591fd08 100644 --- a/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart +++ b/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart @@ -30,7 +30,7 @@ class SpinForAruco extends RoverState { @override void update() { - drive.setThrottle(config.turnThrottle); + drive.setThrottle(0.05); if (collection.video.getArucoDetection( arucoId, @@ -43,16 +43,16 @@ class SpinForAruco extends RoverState { drive.spinLeft(); - if ((collection.imu.heading - _startOrientation).clampHalfAngle().abs() > + if ((collection.imu.heading - _startOrientation).clampHalfAngle() > 175) { _rotated180 = true; } - if (_rotated180 && - collection.imu.isNear(Orientation(z: _startOrientation))) { - controller.popState(); - return; - } + // if (_rotated180 && + // collection.imu.isNear(Orientation(z: _startOrientation))) { + // controller.popState(); + // return; + // } } @override From b81cc10e79d344ae36b3807a964e7784710ccaef Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Sun, 22 Mar 2026 12:31:21 -0400 Subject: [PATCH 6/6] Formatting fixes --- .../lib/src/orchestrator/rover_orchestrator.dart | 5 +---- .../state_machine/rover_states/spin_for_aruco.dart | 13 ++++++------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/autonomy/lib/src/orchestrator/rover_orchestrator.dart b/autonomy/lib/src/orchestrator/rover_orchestrator.dart index 11a432ed..3c86edd7 100644 --- a/autonomy/lib/src/orchestrator/rover_orchestrator.dart +++ b/autonomy/lib/src/orchestrator/rover_orchestrator.dart @@ -292,10 +292,7 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter { FunctionalState( controller, onEnter: (controller) { - if (!collection.gps.isNear( - command.destination, - 3.25, - )) { + if (!collection.gps.isNear(command.destination, 3.25)) { // handle error driveToDestinationFailed = true; } else { diff --git a/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart b/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart index 7591fd08..56ccd3e9 100644 --- a/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart +++ b/autonomy/lib/src/state_machine/rover_states/spin_for_aruco.dart @@ -43,16 +43,15 @@ class SpinForAruco extends RoverState { drive.spinLeft(); - if ((collection.imu.heading - _startOrientation).clampHalfAngle() > - 175) { + if ((collection.imu.heading - _startOrientation).clampHalfAngle() > 175) { _rotated180 = true; } - // if (_rotated180 && - // collection.imu.isNear(Orientation(z: _startOrientation))) { - // controller.popState(); - // return; - // } + if (_rotated180 && + collection.imu.isNear(Orientation(z: _startOrientation))) { + controller.popState(); + return; + } } @override