From 389b3f5ecd777708ccfa20b1d957308657345e22 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Mon, 4 Nov 2024 19:36:12 -0500
Subject: [PATCH 01/21] Initial test of diagonal movement

---
 bin/task.dart                     |  6 +-
 lib/src/interfaces/a_star.dart    | 91 +++++++++++++++++++++++++++++--
 lib/src/interfaces/drive.dart     | 46 +++++++++++++++-
 lib/src/interfaces/gps_utils.dart | 12 ++++
 lib/src/rover/drive.dart          |  6 ++
 lib/src/rover/drive/sensor.dart   | 12 ++++
 lib/src/rover/drive/timed.dart    | 12 ++++
 lib/src/rover/orchestrator.dart   | 10 ++--
 lib/src/rover/sensorless.dart     | 12 ++++
 lib/src/simulator/drive.dart      | 19 +++++++
 pubspec.lock                      | 16 ++++--
 test/path_test.dart               | 12 ++--
 12 files changed, 229 insertions(+), 25 deletions(-)

diff --git a/bin/task.dart b/bin/task.dart
index a267658..798289b 100644
--- a/bin/task.dart
+++ b/bin/task.dart
@@ -5,7 +5,6 @@ import "package:burt_network/logging.dart";
 
 final chair = (2, 0).toGps();
 final obstacles = <SimulatedObstacle>[
-  SimulatedObstacle(coordinates: (2, 0).toGps(), radius: 3),
   SimulatedObstacle(coordinates: (6, -1).toGps(), radius: 3),
   SimulatedObstacle(coordinates: (6, 1).toGps(), radius: 3),
 ];
@@ -17,9 +16,10 @@ void main() async {
   simulator.detector = DetectorSimulator(collection: simulator, obstacles: obstacles);
   simulator.pathfinder = RoverPathfinder(collection: simulator);
   simulator.orchestrator = RoverOrchestrator(collection: simulator);
-  simulator.drive = RoverDrive(collection: simulator, useImu: true, useGps: false);
+  // simulator.drive = RoverDrive(collection: simulator, useImu: true, useGps: false);
   simulator.gps = GpsSimulator(collection: simulator);
-//  simulator.drive = DriveSimulator(collection: simulator);
+  simulator.imu = ImuSimulator(collection: simulator);
+  simulator.drive = DriveSimulator(collection: simulator, shouldDelay: true);
   await simulator.init();
   await simulator.imu.waitForValue();
 //	await simulator.drive.faceNorth();
diff --git a/lib/src/interfaces/a_star.dart b/lib/src/interfaces/a_star.dart
index e36d4bf..3f7b9e6 100644
--- a/lib/src/interfaces/a_star.dart
+++ b/lib/src/interfaces/a_star.dart
@@ -1,3 +1,5 @@
+import "dart:math";
+
 import "package:a_star/a_star.dart";
 
 import "package:burt_network/generated.dart";
@@ -37,10 +39,12 @@ class AutonomyAStarState extends AStarState<AutonomyAStarState> {
     DriveDirection.left => "Turn left to face $direction",
     DriveDirection.right => "Turn right to face $direction",
     DriveDirection.stop => "Start/Stop at ${position.prettyPrint()}",
+    DriveDirection.forwardLeft => "Turn 45 degrees left to face $direction",
+    DriveDirection.forwardRight => "Turn 45 degrees right to face $direction",
   };
 
   @override
-  double heuristic() => position.manhattanDistance(goal);
+  double heuristic() => position.distanceTo(goal);
 
   @override
   String hash() => "${position.prettyPrint()} ($orientation)";
@@ -54,13 +58,88 @@ class AutonomyAStarState extends AStarState<AutonomyAStarState> {
     orientation: orientation, 
     direction: direction,
     goal: goal, 
-    depth: direction == DriveDirection.forward ? depth + 1 : depth + 2,
+    depth: (direction == DriveDirection.forward)
+        ? depth + 1
+        : (direction == DriveDirection.forwardLeft || direction == DriveDirection.forwardRight)
+            ? depth + sqrt2
+            : depth + 2,
   );
 
+  bool drivingThroughObstacle(AutonomyAStarState state) {
+    final isTurn = state.direction != DriveDirection.forward;
+    final isQuarterTurn = state.direction == DriveDirection.forwardLeft || state.direction == DriveDirection.forwardRight;
+
+    // Forward drive across the perpendicular axis
+    if (!isTurn && state.orientation.angle.abs() % 90 == 0) {
+      return false;
+    }
+
+    // Not encountering any sort of diagonal angle
+    if (isTurn && isQuarterTurn && state.orientation.angle.abs() % 90 == 0) {
+      return false;
+    }
+
+    // No diagonal movement, won't drive between obstacles
+    if (!isQuarterTurn && orientation.angle.abs() % 90 == 0) {
+      return false;
+    }
+
+    DriveOrientation orientation1;
+    DriveOrientation orientation2;
+
+    if (!isTurn) {
+      orientation1 = state.orientation.turnQuarterLeft();
+      orientation2 = state.orientation.turnQuarterRight();
+    } else if (isQuarterTurn) {
+      orientation1 = orientation;
+      orientation2 = (state.direction == DriveDirection.forwardLeft)
+          ? orientation1.turnLeft()
+          : orientation1.turnRight();
+    } else {
+      orientation1 = (state.direction == DriveDirection.left)
+          ? orientation.turnQuarterLeft()
+          : orientation.turnQuarterRight();
+      orientation2 = (state.direction == DriveDirection.left)
+          ? state.orientation.turnQuarterLeft()
+          : state.orientation.turnQuarterRight();
+    }
+
+    // Since the state being passed has a position of moving after the
+    // turn, we have to check the position of where it started
+    return collection.pathfinder.isObstacle(
+          position.goForward(orientation1),
+        ) ||
+        collection.pathfinder.isObstacle(
+          position.goForward(orientation2),
+        );
+  }
+
   @override
   Iterable<AutonomyAStarState> expand() => [
-    copyWith(direction: DriveDirection.forward, orientation: orientation, position: position.goForward(orientation)),
-    copyWith(direction: DriveDirection.left, orientation: orientation.turnLeft(), position: position),
-    copyWith(direction: DriveDirection.right, orientation: orientation.turnRight(), position: position),    
-  ].where((state) => !collection.pathfinder.isObstacle(state.position));
+        copyWith(
+          direction: DriveDirection.forward,
+          orientation: orientation,
+          position: position.goForward(orientation),
+        ),
+        copyWith(
+          direction: DriveDirection.left,
+          orientation: orientation.turnLeft(),
+          position: position,
+        ),
+        copyWith(
+          direction: DriveDirection.right,
+          orientation: orientation.turnRight(),
+          position: position,
+        ),
+        copyWith(
+          direction: DriveDirection.forwardLeft,
+          orientation: orientation.turnQuarterLeft(),
+          position: position,
+        ),
+        copyWith(
+          direction: DriveDirection.forwardRight,
+          orientation: orientation.turnQuarterRight(),
+          position: position,
+        ),
+      ].where((state) => !collection.pathfinder.isObstacle(state.position) && !drivingThroughObstacle(state));
 }
diff --git a/lib/src/interfaces/drive.dart b/lib/src/interfaces/drive.dart
index d7dd8b2..03c8d8e 100644
--- a/lib/src/interfaces/drive.dart
+++ b/lib/src/interfaces/drive.dart
@@ -5,14 +5,22 @@ enum DriveDirection {
   forward, 
   left,
   right,
-  stop,
+  forwardLeft,
+  forwardRight,
+  stop;
+
+  bool get isTurn => this != forward && this != stop;
 }
 
 enum DriveOrientation {
   north(0),
   west(90),
   south(180),
-  east(270);
+  east(270),
+  northEast(360 - 45),
+  northWest(45),
+  southEast(180 + 45),
+  southWest(180 - 45);
 
   final int angle;
   const DriveOrientation(this.angle);
@@ -30,6 +38,10 @@ enum DriveOrientation {
     west => south,
     south => east,
     east => north,
+    northEast => northWest,
+    northWest => southWest,
+    southWest => southEast,
+    southEast => northEast,
   };
 
   DriveOrientation turnRight() => switch (this) {
@@ -37,6 +49,32 @@ enum DriveOrientation {
     west => north,
     south => west,
     east => south,
+    northEast => southEast,
+    southEast => southWest,
+    southWest => northWest,
+    northWest => northEast,
+  };
+
+  DriveOrientation turnQuarterLeft() => switch (this) {
+    north => northWest,
+    northWest => west,
+    west => southWest,
+    southWest => south,
+    south => southEast,
+    southEast => east,
+    east => northEast,
+    northEast => north,
+  };
+
+  DriveOrientation turnQuarterRight() => switch (this) {
+    north => northEast,
+    northEast => east,
+    east => southEast,
+    southEast => south,
+    south => southWest,
+    southWest => west,
+    west => northWest,
+    northWest => north,
   };
 }
 
@@ -49,6 +87,8 @@ abstract class DriveInterface extends Service {
     DriveDirection.forward => await goForward(),
     DriveDirection.left => await turnLeft(),
     DriveDirection.right => await turnRight(),
+    DriveDirection.forwardLeft => await turnQuarterLeft(),
+    DriveDirection.forwardRight => await turnQuarterRight(),
     DriveDirection.stop => await stop(),
   };
 
@@ -57,6 +97,8 @@ abstract class DriveInterface extends Service {
   Future<void> goForward();
   Future<void> turnLeft();
   Future<void> turnRight();
+  Future<void> turnQuarterLeft();
+  Future<void> turnQuarterRight();
   Future<void> stop();
 
   Future<void> faceDirection(DriveOrientation orientation) async {
diff --git a/lib/src/interfaces/gps_utils.dart b/lib/src/interfaces/gps_utils.dart
index a303a6a..8a04356 100644
--- a/lib/src/interfaces/gps_utils.dart
+++ b/lib/src/interfaces/gps_utils.dart
@@ -21,6 +21,14 @@ extension GpsUtils on GpsCoordinates {
     GpsCoordinates(latitude: 1 * latitudePerMeter);
   static GpsCoordinates get south => 
     GpsCoordinates(latitude: -1 * latitudePerMeter);
+  static GpsCoordinates get northEast => GpsCoordinates(
+      latitude: 1 * latitudePerMeter, longitude: -1 / metersPerLongitude);
+  static GpsCoordinates get northWest => GpsCoordinates(
+      latitude: 1 * latitudePerMeter, longitude: 1 / metersPerLongitude);
+  static GpsCoordinates get southEast => GpsCoordinates(
+      latitude: -1 * latitudePerMeter, longitude: -1 / metersPerLongitude);
+  static GpsCoordinates get southWest => GpsCoordinates(
+      latitude: -1 * latitudePerMeter, longitude: 1 / metersPerLongitude);
 
   // Taken from https://stackoverflow.com/a/39540339/9392211
   // static const metersPerLatitude = 111.32 * 1000;  // 111.32 km
@@ -58,5 +66,9 @@ extension GpsUtils on GpsCoordinates {
     DriveOrientation.south => GpsUtils.south,
     DriveOrientation.west => GpsUtils.west,
     DriveOrientation.east => GpsUtils.east,
+    DriveOrientation.northEast => GpsUtils.northEast,
+    DriveOrientation.northWest => GpsUtils.northWest,
+    DriveOrientation.southEast => GpsUtils.southEast,
+    DriveOrientation.southWest => GpsUtils.southWest,
   };
 }
diff --git a/lib/src/rover/drive.dart b/lib/src/rover/drive.dart
index 7cef09e..7235668 100644
--- a/lib/src/rover/drive.dart
+++ b/lib/src/rover/drive.dart
@@ -64,4 +64,10 @@ class RoverDrive extends DriveInterface {
 
   @override
   Future<void> turnRight() => useImu ? sensorDrive.turnRight() : timedDrive.turnRight();
+
+  @override
+  Future<void> turnQuarterLeft() => useImu ? sensorDrive.turnQuarterLeft() : timedDrive.turnQuarterLeft();
+
+  @override
+  Future<void> turnQuarterRight() => useImu ? sensorDrive.turnQuarterRight() : timedDrive.turnQuarterRight();
 }
diff --git a/lib/src/rover/drive/sensor.dart b/lib/src/rover/drive/sensor.dart
index 7a6ea93..7611c28 100644
--- a/lib/src/rover/drive/sensor.dart
+++ b/lib/src/rover/drive/sensor.dart
@@ -104,6 +104,18 @@ class SensorDrive extends DriveInterface with RoverMotors {
 	this.orientation = this.orientation.turnRight();
   }
 
+  @override
+  Future<void> turnQuarterLeft() {
+    // TODO: implement turnQuarterLeft
+    throw UnimplementedError();
+  }
+
+  @override
+  Future<void> turnQuarterRight() {
+    // TODO: implement turnQuarterRight
+    throw UnimplementedError();
+  }
+
   @override
   Future<bool> spinForAruco() async {
     for (var i = 0; i < 16; i++) {
diff --git a/lib/src/rover/drive/timed.dart b/lib/src/rover/drive/timed.dart
index 142a1ff..3921bdc 100644
--- a/lib/src/rover/drive/timed.dart
+++ b/lib/src/rover/drive/timed.dart
@@ -48,4 +48,16 @@ class TimedDrive extends DriveInterface with RoverMotors {
     await Future<void>.delayed(turnDelay);
     await stop();
   }
+
+  @override
+  Future<void> turnQuarterLeft() {
+    // TODO: implement turnQuarterLeft
+    throw UnimplementedError();
+  }
+
+  @override
+  Future<void> turnQuarterRight() {
+    // TODO: implement turnQuarterRight
+    throw UnimplementedError();
+  }
 }
diff --git a/lib/src/rover/orchestrator.dart b/lib/src/rover/orchestrator.dart
index 6a88b0d..2000093 100644
--- a/lib/src/rover/orchestrator.dart
+++ b/lib/src/rover/orchestrator.dart
@@ -39,11 +39,13 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
     collection.logger.info("Got GPS Task: Go to ${destination.prettyPrint()}");
     collection.logger.debug("Currently at ${collection.gps.coordinates.prettyPrint()}");
     collection.drive.setLedStrip(ProtoColor.RED);
+    collection.detector.findObstacles();
+    // await collection.drive.faceNorth();
     while (!collection.gps.coordinates.isNear(destination)) {
       // Calculate a path
       collection.logger.debug("Finding a path");
       currentState = AutonomyState.PATHING;
-      await collection.drive.faceNorth();
+      // await collection.drive.faceNorth();
       final path = collection.pathfinder.getPath(destination);
       currentPath = path;  // also use local variable path for promotion
       if (path == null) {
@@ -61,13 +63,13 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
         collection.logger.debug(step.toString());
       }
       currentState = AutonomyState.DRIVING;
-      var count = 0;
+      // var count = 0;
       for (final state in path) {
         collection.logger.debug(state.toString());
         await collection.drive.goDirection(state.direction);
         traversed.add(state.position);
-        if (state.direction != DriveDirection.forward) continue;
-        if (count++ == 5) break;
+        // if (state.direction != DriveDirection.forward) continue;
+        // if (count++ == 5) break;
         final foundObstacle = collection.detector.findObstacles();
         if (foundObstacle) {
           collection.logger.debug("Found an obstacle. Recalculating path...");
diff --git a/lib/src/rover/sensorless.dart b/lib/src/rover/sensorless.dart
index 2beab09..fc07345 100644
--- a/lib/src/rover/sensorless.dart
+++ b/lib/src/rover/sensorless.dart
@@ -61,4 +61,16 @@ class SensorlessDrive extends DriveInterface {
     await simulatedDrive.turnRight();
     await realDrive.turnRight();
   }
+
+  @override
+  Future<void> turnQuarterLeft() {
+    // TODO: implement turnQuarterLeft
+    throw UnimplementedError();
+  }
+
+  @override
+  Future<void> turnQuarterRight() {
+    // TODO: implement turnQuarterRight
+    throw UnimplementedError();
+  }
 }
diff --git a/lib/src/simulator/drive.dart b/lib/src/simulator/drive.dart
index 015dc1a..b1b04d8 100644
--- a/lib/src/simulator/drive.dart
+++ b/lib/src/simulator/drive.dart
@@ -29,6 +29,7 @@ class DriveSimulator extends DriveInterface {
   @override
   Future<void> faceNorth() async {
     collection.imu.update(OrientationUtils.north);
+    if (shouldDelay) await Future<void>.delayed(const Duration(milliseconds: 500));
   }
 
   @override
@@ -49,6 +50,24 @@ class DriveSimulator extends DriveInterface {
     if (shouldDelay) await Future<void>.delayed(const Duration(milliseconds: 500));
   }
 
+  @override
+  Future<void> turnQuarterLeft() async {
+    collection.logger.debug("Turning quarter left");
+    final heading = collection.imu.heading;
+    final orientation = Orientation(z: heading + 45).clampHeading();
+    collection.imu.update(orientation);
+    if (shouldDelay) await Future<void>.delayed(const Duration(milliseconds: 500));
+  }
+
+  @override
+  Future<void> turnQuarterRight() async {
+    collection.logger.debug("Turning quarter right");
+    final heading = collection.imu.heading;
+    final orientation = Orientation(z: heading - 45).clampHeading();
+    collection.imu.update(orientation);
+    if (shouldDelay) await Future<void>.delayed(const Duration(milliseconds: 500));
+  }
+
   @override
   Future<void> stop() async => collection.logger.debug("Stopping");
 }
diff --git a/pubspec.lock b/pubspec.lock
index 0f65b3b..7366b66 100644
--- a/pubspec.lock
+++ b/pubspec.lock
@@ -57,9 +57,11 @@ packages:
   burt_network:
     dependency: "direct main"
     description:
-      path: "../burt_network"
-      relative: true
-    source: path
+      path: "."
+      ref: HEAD
+      resolved-ref: ceeb9602a332d612bd9392d2b865403eea76b465
+      url: "https://github.com/BinghamtonRover/Dart-Networking.git"
+    source: git
     version: "2.0.0"
   collection:
     dependency: transitive
@@ -240,9 +242,11 @@ packages:
   opencv_ffi:
     dependency: "direct main"
     description:
-      path: "../opencv_ffi"
-      relative: true
-    source: path
+      path: "."
+      ref: HEAD
+      resolved-ref: fb721ce518faa62b470c73ae58edfe825a5f9052
+      url: "https://github.com/BinghamtonRover/OpenCV-FFI.git"
+    source: git
     version: "1.2.0"
   package_config:
     dependency: transitive
diff --git a/test/path_test.dart b/test/path_test.dart
index e5be9d4..499fe46 100644
--- a/test/path_test.dart
+++ b/test/path_test.dart
@@ -33,15 +33,16 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
 
     var turnCount = 0;
     for (final step in path) {
-      if (step.direction == DriveDirection.left || step.direction == DriveDirection.right) {
+      if (step.direction.isTurn) {
         turnCount++;
       }
       simulator.logger.trace(step.toString());
     }
 
     // start + 5 forward + 1 turn + 5 right = 12 steps
+    // start + quarter turn left + 7 forward = 8 steps
     expect(turnCount, 1);
-    expect(path.length, 11);
+    expect(path.length, 7);
     
     GpsUtils.maxErrorMeters = oldError;
   });
@@ -69,13 +70,16 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
     simulator.pathfinder.recordObstacle((1, 0).toGps());
     simulator.pathfinder.recordObstacle((2, 0).toGps());
     final path = simulator.pathfinder.getPath(destination);
-    expect(path, isNotNull); if (path == null) return;
+    expect(path, isNotNull);
+    if (path == null) {
+      return;
+    }
     expect(path, isNotEmpty);
     for (final step in path) {
       simulator.logger.trace(step.toString());
       expect(simulator.pathfinder.isObstacle(step.position), isFalse);
     }
-    expect(path.length, 10, reason: "1 Stop + 5 detour + 4 forward = 10 steps total");
+    expect(path.length, 10, reason: "1 turn + 1 forward + 1 turn + 4 forward + 1 45 degree turn + 1 forward + 1 stop = 10 steps total");
     await simulator.drive.followPath(path);
     expect(simulator.gps.isNear(destination), isTrue);
     await simulator.dispose();

From 3fc237c1267dff7f05b4ee9a1c8954cb33006afa Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Tue, 5 Nov 2024 09:08:44 -0500
Subject: [PATCH 02/21] Added turning logic to sensor and timed drive

---
 lib/src/rover/drive/sensor.dart | 70 +++++++++++++++++++++++----------
 lib/src/rover/drive/timed.dart  | 16 +++++---
 lib/src/rover/sensorless.dart   | 12 +++---
 3 files changed, 65 insertions(+), 33 deletions(-)

diff --git a/lib/src/rover/drive/sensor.dart b/lib/src/rover/drive/sensor.dart
index 7611c28..b019a87 100644
--- a/lib/src/rover/drive/sensor.dart
+++ b/lib/src/rover/drive/sensor.dart
@@ -69,16 +69,16 @@ class SensorDrive extends DriveInterface with RoverMotors {
 
   @override
   Future<void> turnLeft() async {
-      await collection.imu.waitForValue();
+    await collection.imu.waitForValue();
 
-	if (collection.imu.orientation == null) {
-		await faceNorth();
-		await faceDirection(this.orientation);
-	}
-      await collection.imu.waitForValue();
+    if (collection.imu.orientation == null) {
+      await faceNorth();
+      await faceDirection(this.orientation);
+    }
+    await collection.imu.waitForValue();
 
     final orientation = collection.imu.orientation;
-    final destination = orientation!.turnLeft();  // do NOT clamp!
+    final destination = orientation!.turnLeft(); // do NOT clamp!
     print("Going from ${orientation} to ${destination}");
     setThrottle(maxThrottle);
     setSpeeds(left: -1, right: 1);
@@ -89,31 +89,59 @@ class SensorDrive extends DriveInterface with RoverMotors {
 
   @override
   Future<void> turnRight() async {
-      await collection.imu.waitForValue();
-        if (collection.imu.orientation == null) {
-                await faceNorth();
-                await faceDirection(this.orientation);
-        }
-      await collection.imu.waitForValue();
+    await collection.imu.waitForValue();
+    if (collection.imu.orientation == null) {
+      await faceNorth();
+      await faceDirection(this.orientation);
+    }
+    await collection.imu.waitForValue();
     final orientation = collection.imu.orientation;
-    final destination = orientation!.turnRight();  // do NOT clamp!
+    final destination = orientation!.turnRight(); // do NOT clamp!
     setThrottle(maxThrottle);
     setSpeeds(left: 1, right: -1);
     await waitFor(() => collection.imu.orientation == destination);
     await stop();
-	this.orientation = this.orientation.turnRight();
+    this.orientation = this.orientation.turnRight();
   }
 
   @override
-  Future<void> turnQuarterLeft() {
-    // TODO: implement turnQuarterLeft
-    throw UnimplementedError();
+  Future<void> turnQuarterLeft() async {
+    await collection.imu.waitForValue();
+
+    if (collection.imu.orientation == null) {
+      await faceNorth();
+      await faceDirection(this.orientation);
+    }
+    await collection.imu.waitForValue();
+
+    final orientation = collection.imu.orientation;
+    final destination = orientation!.turnQuarterLeft(); // do NOT clamp!
+    print("Going from ${orientation} to ${destination}");
+    setThrottle(maxThrottle);
+    setSpeeds(left: -1, right: 1);
+    await waitFor(() => collection.imu.orientation == destination);
+    await stop();
+    this.orientation = this.orientation.turnQuarterLeft();
   }
 
   @override
-  Future<void> turnQuarterRight() {
-    // TODO: implement turnQuarterRight
-    throw UnimplementedError();
+  Future<void> turnQuarterRight() async {
+    await collection.imu.waitForValue();
+
+    if (collection.imu.orientation == null) {
+      await faceNorth();
+      await faceDirection(this.orientation);
+    }
+    await collection.imu.waitForValue();
+
+    final orientation = collection.imu.orientation;
+    final destination = orientation!.turnQuarterRight(); // do NOT clamp!
+    print("Going from ${orientation} to ${destination}");
+    setThrottle(maxThrottle);
+    setSpeeds(left: 1, right: -1);
+    await waitFor(() => collection.imu.orientation == destination);
+    await stop();
+    this.orientation = this.orientation.turnQuarterRight();
   }
 
   @override
diff --git a/lib/src/rover/drive/timed.dart b/lib/src/rover/drive/timed.dart
index 3921bdc..0d03a22 100644
--- a/lib/src/rover/drive/timed.dart
+++ b/lib/src/rover/drive/timed.dart
@@ -50,14 +50,18 @@ class TimedDrive extends DriveInterface with RoverMotors {
   }
 
   @override
-  Future<void> turnQuarterLeft() {
-    // TODO: implement turnQuarterLeft
-    throw UnimplementedError();
+  Future<void> turnQuarterLeft() async {
+    setThrottle(turnThrottle);
+    setSpeeds(left: -1, right: 1);
+    await Future<void>.delayed(turnDelay * 0.5);
+    await stop();
   }
 
   @override
-  Future<void> turnQuarterRight() {
-    // TODO: implement turnQuarterRight
-    throw UnimplementedError();
+  Future<void> turnQuarterRight() async {
+    setThrottle(turnThrottle);
+    setSpeeds(left: 1, right: -1);
+    await Future<void>.delayed(turnDelay * 0.5);
+    await stop();
   }
 }
diff --git a/lib/src/rover/sensorless.dart b/lib/src/rover/sensorless.dart
index fc07345..7a9b7b4 100644
--- a/lib/src/rover/sensorless.dart
+++ b/lib/src/rover/sensorless.dart
@@ -63,14 +63,14 @@ class SensorlessDrive extends DriveInterface {
   }
 
   @override
-  Future<void> turnQuarterLeft() {
-    // TODO: implement turnQuarterLeft
-    throw UnimplementedError();
+  Future<void> turnQuarterLeft() async {
+    await simulatedDrive.turnQuarterLeft();
+    await realDrive.turnQuarterLeft();
   }
 
   @override
-  Future<void> turnQuarterRight() {
-    // TODO: implement turnQuarterRight
-    throw UnimplementedError();
+  Future<void> turnQuarterRight() async {
+    await simulatedDrive.turnQuarterRight();
+    await realDrive.turnQuarterRight();
   }
 }

From 131be910f7b88e2abb5744586b76147c06fbef5d Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Tue, 5 Nov 2024 09:27:39 -0500
Subject: [PATCH 03/21] More docs

---
 lib/src/interfaces/a_star.dart | 32 +++++++++++++++++++++++++++-----
 lib/src/interfaces/drive.dart  |  2 ++
 2 files changed, 29 insertions(+), 5 deletions(-)

diff --git a/lib/src/interfaces/a_star.dart b/lib/src/interfaces/a_star.dart
index 3f7b9e6..b738c47 100644
--- a/lib/src/interfaces/a_star.dart
+++ b/lib/src/interfaces/a_star.dart
@@ -65,37 +65,59 @@ class AutonomyAStarState extends AStarState<AutonomyAStarState> {
             : depth + 2,
   );
 
+  /// Returns whether or not the rover will drive between or right next to an obstacle diagonally<br/>
+  /// <br/>
+  /// Case 1:<br/>
+  /// 0 X<br/>
+  /// X R<br/>
+  /// Assuming the rover is facing 0 and trying to drive forward, will return false<br/>
+  /// <br/>
+  /// Case 2:<br/>
+  /// 0 X<br/>
+  /// X R<br/>
+  /// Assuming the rover is facing north and trying to turn 45 degrees left, will return false<br/>
+  /// <br/>
+  /// Case 3:<br/>
+  /// 0 X<br/>
+  /// 0 R<br/>
+  /// If the rover is facing left but trying to turn 45 degrees right, will return false<br/>
+  /// <br/>
+  /// Case 4:<br/>
+  /// 0 X 0<br/>
+  /// 0 R 0<br/>
+  /// If the rover is facing northeast to 0 and trying to turn left, will return false
   bool drivingThroughObstacle(AutonomyAStarState state) {
     final isTurn = state.direction != DriveDirection.forward;
     final isQuarterTurn = state.direction == DriveDirection.forwardLeft || state.direction == DriveDirection.forwardRight;
 
     // Forward drive across the perpendicular axis
-    if (!isTurn && state.orientation.angle.abs() % 90 == 0) {
+    if (!isTurn && state.orientation.isPerpendicular) {
       return false;
     }
 
     // Not encountering any sort of diagonal angle
-    if (isTurn && isQuarterTurn && state.orientation.angle.abs() % 90 == 0) {
+    if (isTurn && isQuarterTurn && state.orientation.isPerpendicular) {
       return false;
     }
 
     // No diagonal movement, won't drive between obstacles
-    if (!isQuarterTurn && orientation.angle.abs() % 90 == 0) {
+    if (!isQuarterTurn && orientation.isPerpendicular) {
       return false;
     }
 
     DriveOrientation orientation1;
     DriveOrientation orientation2;
 
+    // Case 1, trying to drive while facing a 45 degree angle
     if (!isTurn) {
       orientation1 = state.orientation.turnQuarterLeft();
       orientation2 = state.orientation.turnQuarterRight();
-    } else if (isQuarterTurn) {
+    } else if (isQuarterTurn) { // Case 2 and Case 3
       orientation1 = orientation;
       orientation2 = (state.direction == DriveDirection.forwardLeft)
           ? orientation1.turnLeft()
           : orientation1.turnRight();
-    } else {
+    } else { // Case 4
       orientation1 = (state.direction == DriveDirection.left)
           ? orientation.turnQuarterLeft()
           : orientation.turnQuarterRight();
diff --git a/lib/src/interfaces/drive.dart b/lib/src/interfaces/drive.dart
index 03c8d8e..80439b7 100644
--- a/lib/src/interfaces/drive.dart
+++ b/lib/src/interfaces/drive.dart
@@ -33,6 +33,8 @@ enum DriveOrientation {
     return null;
   }
 
+  bool get isPerpendicular => angle.abs() % 90 == 0;
+
   DriveOrientation turnLeft() => switch (this) {
     north => west,
     west => south,

From f7e34f0c1fbde2c20fa4f3331c2c3b74db3ccde5 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Sun, 24 Nov 2024 11:26:48 -0500
Subject: [PATCH 04/21] Added network detector

---
 lib/src/simulator/network_detector.dart | 47 +++++++++++++++++++++++++
 1 file changed, 47 insertions(+)
 create mode 100644 lib/src/simulator/network_detector.dart

diff --git a/lib/src/simulator/network_detector.dart b/lib/src/simulator/network_detector.dart
new file mode 100644
index 0000000..804519f
--- /dev/null
+++ b/lib/src/simulator/network_detector.dart
@@ -0,0 +1,47 @@
+import "package:autonomy/autonomy.dart";
+import "package:burt_network/burt_network.dart";
+
+class NetworkDetector extends DetectorInterface {
+  final List<GpsCoordinates> _newObstacles = [];
+  bool _hasNewObstacles = false;
+
+  NetworkDetector({required super.collection});
+
+  @override
+  bool canSeeAruco() => false;
+
+  @override
+  Future<void> dispose() async {}
+
+  @override
+  bool findObstacles() {
+    for (final obstacle in _newObstacles) {
+      collection.pathfinder.recordObstacle(obstacle);
+    }
+    final newObstacles = _hasNewObstacles;
+    _hasNewObstacles = false;
+
+    _newObstacles.clear();
+
+    return newObstacles;
+  }
+
+  @override
+  Future<bool> init() async {
+    collection.server.messages.onMessage(
+        name: AutonomyData().messageName,
+        constructor: AutonomyData.fromBuffer,
+        callback: _onDataReceived);
+    return true;
+  }
+
+  void _onDataReceived(AutonomyData data) {
+    if (data.obstacles.isNotEmpty) {
+      _newObstacles.addAll(data.obstacles);
+      _hasNewObstacles = true;
+    }
+  }
+
+  @override
+  bool isOnSlope() => false;
+}

From e3e11736af39d4ca210e64ea4e31c3cee34c7d2b Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Sun, 24 Nov 2024 11:29:54 -0500
Subject: [PATCH 05/21] Refactored movement units

---
 bin/latlong.dart                  |  4 +--
 lib/interfaces.dart               |  1 +
 lib/src/interfaces/a_star.dart    |  4 +--
 lib/src/interfaces/gps_utils.dart | 49 +++++++++++++------------------
 lib/src/rover/orchestrator.dart   |  9 +++---
 5 files changed, 31 insertions(+), 36 deletions(-)

diff --git a/bin/latlong.dart b/bin/latlong.dart
index eb5ce07..2c8bbf5 100644
--- a/bin/latlong.dart
+++ b/bin/latlong.dart
@@ -9,14 +9,14 @@ void printInfo(String name, double latitude) {
   GpsInterface.currentLatitude = latitude;
   print("At $name:");
   print("  There are ${GpsUtils.metersPerLongitude.toStringAsFixed(2)} meters per 1 degree of longitude");
-  print("  Our max error in longitude would be ${GpsUtils.epsilonLongitude} degrees");
+  print("  Our max error in longitude would be ${GpsUtils.epsilonLongitude.toStringAsFixed(20)} degrees");
   final isWithinRange = GpsInterface.gpsError <= GpsUtils.epsilonLongitude;
   print("  Our GPS has ${GpsInterface.gpsError} degrees of error, so this would ${isWithinRange ? 'work' : 'not work'}");
 }
 
 void main() {
   print("There are always ${GpsUtils.metersPerLatitude} meters in 1 degree of latitude");
-  print("  So our max error in latitude is always ${GpsUtils.epsilonLatitude} degrees");
+  print("  So our max error in latitude is always ${GpsUtils.epsilonLatitude.toStringAsFixed(20)} degrees");
   printInfo("the equator", 0);
   printInfo("Binghamton", binghamtonLatitude);
   printInfo("Utah", utahLatitude);
diff --git a/lib/interfaces.dart b/lib/interfaces.dart
index bd903f0..a6ecf3b 100644
--- a/lib/interfaces.dart
+++ b/lib/interfaces.dart
@@ -14,3 +14,4 @@ export "src/interfaces/receiver.dart";
 export "src/interfaces/reporter.dart";
 export "src/interfaces/service.dart";
 export "src/interfaces/orchestrator.dart";
+export "package:burt_network/src/utils.dart";
diff --git a/lib/src/interfaces/a_star.dart b/lib/src/interfaces/a_star.dart
index b738c47..07bcf0a 100644
--- a/lib/src/interfaces/a_star.dart
+++ b/lib/src/interfaces/a_star.dart
@@ -50,7 +50,7 @@ class AutonomyAStarState extends AStarState<AutonomyAStarState> {
   String hash() => "${position.prettyPrint()} ($orientation)";
 
   @override
-  bool isGoal() => position.isNear(goal);
+  bool isGoal() => position.isNear(goal, min(GpsUtils.moveLengthMeters, GpsUtils.maxErrorMeters));
 
   AutonomyAStarState copyWith({required DriveDirection direction, required DriveOrientation orientation, required GpsCoordinates position}) => AutonomyAStarState(
     collection: collection,
@@ -62,7 +62,7 @@ class AutonomyAStarState extends AStarState<AutonomyAStarState> {
         ? depth + 1
         : (direction == DriveDirection.forwardLeft || direction == DriveDirection.forwardRight)
             ? depth + sqrt2
-            : depth + 2,
+            : depth + 2 * sqrt2,
   );
 
   /// Returns whether or not the rover will drive between or right next to an obstacle diagonally<br/>
diff --git a/lib/src/interfaces/gps_utils.dart b/lib/src/interfaces/gps_utils.dart
index 8a04356..63b3948 100644
--- a/lib/src/interfaces/gps_utils.dart
+++ b/lib/src/interfaces/gps_utils.dart
@@ -4,38 +4,29 @@ import "dart:math";
 import "package:autonomy/interfaces.dart";
 import "package:burt_network/generated.dart";
 
-extension RecordToGps on (num, num) {
-  GpsCoordinates toGps() => GpsCoordinates(latitude: $1.toDouble() * GpsUtils.latitudePerMeter, longitude: $2.toDouble() * GpsUtils.longitudePerMeter);
-}
-
 extension GpsUtils on GpsCoordinates {
   static double maxErrorMeters = 1;
+  static double moveLengthMeters = 0.5;
   static double get epsilonLatitude => maxErrorMeters * latitudePerMeter; 
-  static double get epsilonLongitude => maxErrorMeters * longitudePerMeter; 
+  static double get epsilonLongitude => maxErrorMeters * longitudePerMeter;
+
+  static double get movementLatitude => moveLengthMeters * latitudePerMeter; 
+  static double get movementLongitude => moveLengthMeters * longitudePerMeter;
 
-  static GpsCoordinates get east => 
-    GpsCoordinates(longitude: -1 / metersPerLongitude);
-  static GpsCoordinates get west => 
-    GpsCoordinates(longitude: 1 / metersPerLongitude);
-  static GpsCoordinates get north => 
-    GpsCoordinates(latitude: 1 * latitudePerMeter);
-  static GpsCoordinates get south => 
-    GpsCoordinates(latitude: -1 * latitudePerMeter);
-  static GpsCoordinates get northEast => GpsCoordinates(
-      latitude: 1 * latitudePerMeter, longitude: -1 / metersPerLongitude);
-  static GpsCoordinates get northWest => GpsCoordinates(
-      latitude: 1 * latitudePerMeter, longitude: 1 / metersPerLongitude);
-  static GpsCoordinates get southEast => GpsCoordinates(
-      latitude: -1 * latitudePerMeter, longitude: -1 / metersPerLongitude);
-  static GpsCoordinates get southWest => GpsCoordinates(
-      latitude: -1 * latitudePerMeter, longitude: 1 / metersPerLongitude);
+  static GpsCoordinates get east => GpsCoordinates(longitude: -movementLongitude);
+  static GpsCoordinates get west => GpsCoordinates(longitude: movementLongitude);
+  static GpsCoordinates get north => GpsCoordinates(latitude: movementLatitude);
+  static GpsCoordinates get south => GpsCoordinates(latitude: -movementLatitude);
+  static GpsCoordinates get northEast => GpsCoordinates(latitude: movementLatitude, longitude: -movementLongitude);
+  static GpsCoordinates get northWest => GpsCoordinates(latitude: movementLatitude, longitude: movementLongitude);
+  static GpsCoordinates get southEast => GpsCoordinates(latitude: -movementLatitude, longitude: -movementLongitude);
+  static GpsCoordinates get southWest => GpsCoordinates(latitude: -movementLatitude, longitude: movementLongitude);
 
   // Taken from https://stackoverflow.com/a/39540339/9392211
-  // static const metersPerLatitude = 111.32 * 1000;  // 111.32 km
-  static const metersPerLatitude = 1;
+  static const metersPerLatitude = 111.32 * 1000;  // 111.32 km
+  // static const metersPerLatitude = 1;
   static const radiansPerDegree = pi / 180;
-  static double get metersPerLongitude => 1;
-//    40075 * cos( GpsInterface.currentLatitude * radiansPerDegree ) / 360 * 1000;
+  static double get metersPerLongitude => 40075 * cos(GpsInterface.currentLatitude * radiansPerDegree) / 360 * 1000.0;
   
   static double get latitudePerMeter => 1 / metersPerLatitude;
   static double get longitudePerMeter => 1 / metersPerLongitude;
@@ -49,9 +40,11 @@ extension GpsUtils on GpsCoordinates {
     (latitude - other.latitude).abs() * metersPerLatitude + 
     (longitude - other.longitude).abs() * metersPerLongitude;
 
-  bool isNear(GpsCoordinates other) => 
-    (latitude - other.latitude).abs() < epsilonLatitude &&
-    (longitude - other.longitude).abs() < epsilonLongitude;
+  bool isNear(GpsCoordinates other, [double? tolerance]) {
+    tolerance ??= maxErrorMeters;
+    return (latitude - other.latitude).abs() < tolerance * latitudePerMeter &&
+        (longitude - other.longitude).abs() < tolerance * longitudePerMeter;
+  }
 
   GpsCoordinates operator +(GpsCoordinates other) => GpsCoordinates(
     latitude: latitude + other.latitude,
diff --git a/lib/src/rover/orchestrator.dart b/lib/src/rover/orchestrator.dart
index 2000093..a8cbe0c 100644
--- a/lib/src/rover/orchestrator.dart
+++ b/lib/src/rover/orchestrator.dart
@@ -38,9 +38,10 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
     final destination = command.destination;
     collection.logger.info("Got GPS Task: Go to ${destination.prettyPrint()}");
     collection.logger.debug("Currently at ${collection.gps.coordinates.prettyPrint()}");
+    traversed.clear();
     collection.drive.setLedStrip(ProtoColor.RED);
     collection.detector.findObstacles();
-    // await collection.drive.faceNorth();
+    await collection.drive.faceNorth();
     while (!collection.gps.coordinates.isNear(destination)) {
       // Calculate a path
       collection.logger.debug("Finding a path");
@@ -57,19 +58,19 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
       }
       // Try to take that path
       final current = collection.gps.coordinates;
-      collection.logger.info("Found a path from ${current.prettyPrint()} to ${destination.prettyPrint()}: ${path.length} steps");
+      collection.logger.debug("Found a path from ${current.prettyPrint()} to ${destination.prettyPrint()}: ${path.length} steps");
       collection.logger.debug("Here is a summary of the path");
       for (final step in path) {
         collection.logger.debug(step.toString());
       }
       currentState = AutonomyState.DRIVING;
-      // var count = 0;
+      var count = 0;
       for (final state in path) {
         collection.logger.debug(state.toString());
         await collection.drive.goDirection(state.direction);
         traversed.add(state.position);
         // if (state.direction != DriveDirection.forward) continue;
-        // if (count++ == 5) break;
+        if (count++ == 5) break;
         final foundObstacle = collection.detector.findObstacles();
         if (foundObstacle) {
           collection.logger.debug("Found an obstacle. Recalculating path...");

From 40b14410efb6d407f8dbc713e6a0b960f9a44c95 Mon Sep 17 00:00:00 2001
From: BinghamtonRover <rover@binghamton.edu>
Date: Sun, 24 Nov 2024 13:06:09 -0500
Subject: [PATCH 06/21] Added tank autonomy file

---
 bin/tank_autonomy.dart | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)
 create mode 100644 bin/tank_autonomy.dart

diff --git a/bin/tank_autonomy.dart b/bin/tank_autonomy.dart
new file mode 100644
index 0000000..98e672f
--- /dev/null
+++ b/bin/tank_autonomy.dart
@@ -0,0 +1,23 @@
+import "dart:io";
+
+import "package:autonomy/interfaces.dart";
+import "package:autonomy/rover.dart";
+import "package:autonomy/simulator.dart";
+import "package:autonomy/src/simulator/network_detector.dart";
+import "package:burt_network/burt_network.dart";
+
+void main() async {
+  ServerUtils.subsystemsDestination = SocketInfo(
+    address: InternetAddress("192.168.1.40"),
+    port: 8001,
+  );
+  final tank = RoverAutonomy();
+  tank.detector = NetworkDetector(collection: tank);
+  tank.gps = GpsSimulator(collection: tank);
+  tank.imu = ImuSimulator(collection: tank);
+  tank.drive = SensorlessDrive(collection: tank, useGps: false, useImu: false);
+  await tank.init();
+  await tank.imu.waitForValue();
+
+  await tank.server.waitForConnection();
+}

From 1a877fe73448e9211cf27e46f8cc27b62e5cf163 Mon Sep 17 00:00:00 2001
From: BinghamtonRover <rover@binghamton.edu>
Date: Mon, 25 Nov 2024 20:49:06 -0500
Subject: [PATCH 07/21] Tested on tank 11/25

---
 bin/tank_autonomy.dart            |  6 ++-
 lib/src/interfaces/a_star.dart    |  2 +-
 lib/src/interfaces/drive.dart     |  4 +-
 lib/src/interfaces/gps_utils.dart |  2 +-
 lib/src/interfaces/imu_utils.dart | 11 ++---
 lib/src/rover/drive/sensor.dart   | 74 ++++++++++++++++++++++---------
 lib/src/rover/drive/timed.dart    | 31 ++++++++++---
 lib/src/rover/orchestrator.dart   |  2 +-
 8 files changed, 93 insertions(+), 39 deletions(-)

diff --git a/bin/tank_autonomy.dart b/bin/tank_autonomy.dart
index 98e672f..dac6948 100644
--- a/bin/tank_autonomy.dart
+++ b/bin/tank_autonomy.dart
@@ -3,6 +3,7 @@
 import "package:autonomy/interfaces.dart";
 import "package:autonomy/rover.dart";
 import "package:autonomy/simulator.dart";
+import "package:autonomy/src/rover/imu.dart";
 import "package:autonomy/src/simulator/network_detector.dart";
 import "package:burt_network/burt_network.dart";
 
@@ -14,8 +15,9 @@ void main() async {
   final tank = RoverAutonomy();
   tank.detector = NetworkDetector(collection: tank);
   tank.gps = GpsSimulator(collection: tank);
-  tank.imu = ImuSimulator(collection: tank);
-  tank.drive = SensorlessDrive(collection: tank, useGps: false, useImu: false);
+  // tank.imu = ImuSimulator(collection: tank);
+  tank.imu = RoverImu(collection: tank);
+  tank.drive = RoverDrive(collection: tank, useGps: false);
   await tank.init();
   await tank.imu.waitForValue();
 
diff --git a/lib/src/interfaces/a_star.dart b/lib/src/interfaces/a_star.dart
index 07bcf0a..a0177d3 100644
--- a/lib/src/interfaces/a_star.dart
+++ b/lib/src/interfaces/a_star.dart
@@ -29,7 +29,7 @@ class AutonomyAStarState extends AStarState<AutonomyAStarState> {
     goal: goal, 
     collection: collection, 
     direction: DriveDirection.stop, 
-    orientation: collection.imu.orientation!, 
+    orientation: collection.imu.orientation ?? DriveOrientation.north, 
     depth: 0,
   );
 
diff --git a/lib/src/interfaces/drive.dart b/lib/src/interfaces/drive.dart
index 80439b7..f9b6c96 100644
--- a/lib/src/interfaces/drive.dart
+++ b/lib/src/interfaces/drive.dart
@@ -1,6 +1,8 @@
 import "package:autonomy/interfaces.dart";
 import "package:burt_network/generated.dart";
 
+const bool isRover = false;
+
 enum DriveDirection {
   forward, 
   left,
@@ -28,7 +30,7 @@ enum DriveOrientation {
   static DriveOrientation? fromRaw(Orientation orientation) {
     // TODO: Make this more precise.
     for (final value in values) {
-      if (orientation.isNear(value.angle.toDouble())) return value;
+      if (orientation.isNear(value.angle.toDouble(), OrientationUtils.orientationEpsilon)) return value;
     }
     return null;
   }
diff --git a/lib/src/interfaces/gps_utils.dart b/lib/src/interfaces/gps_utils.dart
index 63b3948..83d4711 100644
--- a/lib/src/interfaces/gps_utils.dart
+++ b/lib/src/interfaces/gps_utils.dart
@@ -6,7 +6,7 @@ import "package:burt_network/generated.dart";
 
 extension GpsUtils on GpsCoordinates {
   static double maxErrorMeters = 1;
-  static double moveLengthMeters = 0.5;
+  static double moveLengthMeters = 1;
   static double get epsilonLatitude => maxErrorMeters * latitudePerMeter; 
   static double get epsilonLongitude => maxErrorMeters * longitudePerMeter;
 
diff --git a/lib/src/interfaces/imu_utils.dart b/lib/src/interfaces/imu_utils.dart
index 96d928e..c0f8a37 100644
--- a/lib/src/interfaces/imu_utils.dart
+++ b/lib/src/interfaces/imu_utils.dart
@@ -1,7 +1,8 @@
 import "package:burt_network/generated.dart";
 
 extension OrientationUtils on Orientation {
-  static const double epsilon = 10;
+  static const double epsilon = 3.5;
+  static const double orientationEpsilon = 10;
   
   static final north = Orientation(z: 0);
   static final west = Orientation(z: 90);
@@ -19,9 +20,9 @@ extension OrientationUtils on Orientation {
     return Orientation(x: x, y: y, z: adjustedHeading);
   }
 
-  bool isNear(double value) => value > 270 && z < 90
-    ? (z + 360 - value).abs() < epsilon
+  bool isNear(double value, [double tolerance = epsilon]) => value > 270 && z < 90
+    ? (z + 360 - value).abs() < tolerance
     : value < 90 && z > 270 
-      ? (z - value - 360).abs() < epsilon
-      : (z - value).abs() < epsilon;
+      ? (z - value - 360).abs() < tolerance
+      : (z - value).abs() < tolerance;
 }
diff --git a/lib/src/rover/drive/sensor.dart b/lib/src/rover/drive/sensor.dart
index b019a87..adaab3f 100644
--- a/lib/src/rover/drive/sensor.dart
+++ b/lib/src/rover/drive/sensor.dart
@@ -5,7 +5,11 @@ import "motors.dart";
 
 class SensorDrive extends DriveInterface with RoverMotors {
   static const double maxThrottle = 0.1;
-  static const double turnThrottle = 0.1;
+  static const double turnThrottleRover = 0.1;
+  static const double turnThrottleTank = 0.35;
+
+  static double get turnThrottle => isRover ? turnThrottleRover : turnThrottleTank;
+
   static const predicateDelay = Duration(milliseconds: 100);
   static const turnDelay = Duration(milliseconds: 1500);
 
@@ -24,6 +28,12 @@ class SensorDrive extends DriveInterface with RoverMotors {
     }
   }
 
+  Future<void> runFeedback(bool Function() completed, [Duration period = predicateDelay]) async {
+    while (!completed()) {
+      await Future<void>.delayed(period);
+    }
+  }
+
   @override
   Future<bool> init() async => true;
 
@@ -46,11 +56,12 @@ class SensorDrive extends DriveInterface with RoverMotors {
   Future<void> faceNorth() async {
     collection.logger.info("Turning to face north...");
     setThrottle(turnThrottle);
-    setSpeeds(left: -1, right: 1);
-    await waitFor(() {
-      collection.logger.trace("Current heading: ${collection.imu.heading}");
-      return collection.imu.raw.isNear(0);
-    });
+    // setSpeeds(left: -1, right: 1);
+    // await waitFor(() {
+    //   collection.logger.trace("Current heading: ${collection.imu.heading}");
+    //   return collection.imu.raw.isNear(0, OrientationUtils.orientationEpsilon);
+    // });
+    await faceDirection(DriveOrientation.north);
     await stop();
   }
 
@@ -59,10 +70,25 @@ class SensorDrive extends DriveInterface with RoverMotors {
     collection.logger.info("Turning to face $orientation...");
     setThrottle(turnThrottle);
     setSpeeds(left: -1, right: 1);
-    await waitFor(() {
-      collection.logger.trace("Current heading: ${collection.imu.heading}");
-      return collection.imu.raw.isNear(orientation.angle.toDouble());
-    });
+    await runFeedback(
+      () {
+        var delta = orientation.angle.toDouble() - collection.imu.raw.z;
+        if (delta < -180) {
+          delta += 360;
+        } else if (delta > 180) {
+          delta -= 360;
+        }
+
+        if (delta < 0) {
+          setSpeeds(left: 1, right: -1);
+        } else {
+          setSpeeds(left: -1, right: 1);
+        }
+        collection.logger.trace("Current heading: ${collection.imu.heading}");
+        return collection.imu.raw.isNear(orientation.angle.toDouble());
+      },
+      const Duration(milliseconds: 10),
+    );
     await stop();
     await super.faceDirection(orientation);
   }
@@ -80,9 +106,10 @@ class SensorDrive extends DriveInterface with RoverMotors {
     final orientation = collection.imu.orientation;
     final destination = orientation!.turnLeft(); // do NOT clamp!
     print("Going from ${orientation} to ${destination}");
-    setThrottle(maxThrottle);
-    setSpeeds(left: -1, right: 1);
-    await waitFor(() => collection.imu.orientation == destination);
+    setThrottle(turnThrottle);
+    // setSpeeds(left: -1, right: 1);
+    // await waitFor(() => collection.imu.orientation == destination);
+    await faceDirection(destination);
     await stop();
     this.orientation = this.orientation.turnLeft();
   }
@@ -97,9 +124,10 @@ class SensorDrive extends DriveInterface with RoverMotors {
     await collection.imu.waitForValue();
     final orientation = collection.imu.orientation;
     final destination = orientation!.turnRight(); // do NOT clamp!
-    setThrottle(maxThrottle);
-    setSpeeds(left: 1, right: -1);
-    await waitFor(() => collection.imu.orientation == destination);
+    setThrottle(turnThrottle);
+    // setSpeeds(left: 1, right: -1);
+    // await waitFor(() => collection.imu.orientation == destination);
+    await faceDirection(destination);
     await stop();
     this.orientation = this.orientation.turnRight();
   }
@@ -117,9 +145,10 @@ class SensorDrive extends DriveInterface with RoverMotors {
     final orientation = collection.imu.orientation;
     final destination = orientation!.turnQuarterLeft(); // do NOT clamp!
     print("Going from ${orientation} to ${destination}");
-    setThrottle(maxThrottle);
-    setSpeeds(left: -1, right: 1);
-    await waitFor(() => collection.imu.orientation == destination);
+    setThrottle(turnThrottle);
+    // setSpeeds(left: -1, right: 1);
+    // await waitFor(() => collection.imu.orientation == destination);
+    await faceDirection(destination);
     await stop();
     this.orientation = this.orientation.turnQuarterLeft();
   }
@@ -137,9 +166,10 @@ class SensorDrive extends DriveInterface with RoverMotors {
     final orientation = collection.imu.orientation;
     final destination = orientation!.turnQuarterRight(); // do NOT clamp!
     print("Going from ${orientation} to ${destination}");
-    setThrottle(maxThrottle);
-    setSpeeds(left: 1, right: -1);
-    await waitFor(() => collection.imu.orientation == destination);
+    setThrottle(turnThrottle);
+    // setSpeeds(left: 1, right: -1);
+    // await waitFor(() => collection.imu.orientation == destination);
+    await faceDirection(destination);
     await stop();
     this.orientation = this.orientation.turnQuarterRight();
   }
diff --git a/lib/src/rover/drive/timed.dart b/lib/src/rover/drive/timed.dart
index 0d03a22..bba9d6e 100644
--- a/lib/src/rover/drive/timed.dart
+++ b/lib/src/rover/drive/timed.dart
@@ -3,10 +3,23 @@ import "package:autonomy/interfaces.dart";
 import "motors.dart";
 
 class TimedDrive extends DriveInterface with RoverMotors {
-  static const maxThrottle = 0.1;
-  static const turnThrottle = 0.1;
-  static const oneMeterDelay = Duration(milliseconds: 5500);
-  static const turnDelay = Duration(milliseconds: 4500);
+  static const maxThrottleTank = 0.3;
+  static const turnThrottleTank = 0.35;
+
+  static const maxThrottleRover = 0.1;
+  static const turnThrottleRover = 0.1;
+
+  static const oneMeterDelayRover = Duration(milliseconds: 5500);
+  static const turnDelayRover = Duration(milliseconds: 4500);
+
+  static const oneMeterDelayTank = Duration(milliseconds: 2000);
+  static const turnDelayTank = Duration(milliseconds: 1000);
+
+  static double get maxThrottle => isRover ? maxThrottleRover : maxThrottleTank;
+  static double get turnThrottle => isRover ? turnThrottleRover : turnThrottleTank;
+
+  static Duration get oneMeterDelay => isRover ? oneMeterDelayRover : oneMeterDelayTank;
+  static Duration get turnDelay => isRover ? turnDelayRover : turnDelayTank;
 
   TimedDrive({required super.collection});
 
@@ -28,7 +41,13 @@ class TimedDrive extends DriveInterface with RoverMotors {
   @override
   Future<void> goForward() async {
     setThrottle(maxThrottle);
-    setSpeeds(left: 1, right: 1);
+    setSpeeds(left: 1 * 0.9, right: 1);
+    final position = collection.gps.coordinates;
+    final orientation = collection.imu.orientation;
+    if (orientation != null) {
+      final newPosition = position.goForward(orientation);
+      collection.gps.update(newPosition);
+    }
     await Future<void>.delayed(oneMeterDelay);
     await stop();
   }
@@ -36,7 +55,7 @@ class TimedDrive extends DriveInterface with RoverMotors {
   @override
   Future<void> turnLeft() async {
     setThrottle(turnThrottle);
-    setSpeeds(left: -1, right: 1);
+    setSpeeds(left: -1 * 0.9, right: 1);
     await Future<void>.delayed(turnDelay);
     await stop();
   }
diff --git a/lib/src/rover/orchestrator.dart b/lib/src/rover/orchestrator.dart
index a8cbe0c..c33e1e9 100644
--- a/lib/src/rover/orchestrator.dart
+++ b/lib/src/rover/orchestrator.dart
@@ -41,7 +41,7 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
     traversed.clear();
     collection.drive.setLedStrip(ProtoColor.RED);
     collection.detector.findObstacles();
-    await collection.drive.faceNorth();
+    // await collection.drive.faceNorth();
     while (!collection.gps.coordinates.isNear(destination)) {
       // Calculate a path
       collection.logger.debug("Finding a path");

From 315c3ed19cd2637557706405855834c4a6b20740 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Thu, 28 Nov 2024 15:20:10 -0500
Subject: [PATCH 08/21] Fixed all occurances of toGps()

---
 bin/path.dart                      |  2 +-
 lib/src/simulator/pathfinding.dart | 20 ++++++++++----------
 test/network_test.dart             |  2 +-
 test/orchestrator_test.dart        | 30 +++++++++++++++---------------
 test/path_test.dart                | 22 +++++++++++-----------
 test/rover_test.dart               | 14 +++++++-------
 test/sensor_test.dart              | 12 ++++++------
 7 files changed, 51 insertions(+), 51 deletions(-)

diff --git a/bin/path.dart b/bin/path.dart
index cf0b5be..58fe8ba 100644
--- a/bin/path.dart
+++ b/bin/path.dart
@@ -4,7 +4,7 @@ import "package:autonomy/simulator.dart";
 
 void main() {
   GpsUtils.maxErrorMeters = 1;
-  final destination = (1000, 1000).toGps();
+  final destination = (lat: 1000, long: 1000).toGps();
   final simulator = AutonomySimulator();
   simulator.pathfinder = RoverPathfinder(collection: simulator);
   final path = simulator.pathfinder.getPath(destination);
diff --git a/lib/src/simulator/pathfinding.dart b/lib/src/simulator/pathfinding.dart
index 8c64b6a..bce45b6 100644
--- a/lib/src/simulator/pathfinding.dart
+++ b/lib/src/simulator/pathfinding.dart
@@ -11,15 +11,15 @@ class PathfindingSimulator extends PathfindingInterface {
 
   @override
   List<AutonomyAStarState> getPath(GpsCoordinates destination, {bool verbose = false}) => [
-    AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 0).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.right, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 0).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 1).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 2).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.left, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (0, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (1, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.left, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 2).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.forward, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 1).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.right, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (2, 1).toGps(), position: (2, 1).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 0).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.right, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 0).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 1).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 2).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.left, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 1, long: 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.left, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 2).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.forward, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 1).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.right, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 1).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection),
   ];
 }
diff --git a/test/network_test.dart b/test/network_test.dart
index 8e482df..dc5a1fd 100644
--- a/test/network_test.dart
+++ b/test/network_test.dart
@@ -110,7 +110,7 @@ void main() => group("[Network]", tags: ["network"], () {
     expect(simulator.imu.orientation, DriveOrientation.north);
 
     final origin = GpsCoordinates(latitude: 0, longitude: 0);
-    final oneMeter = (1, 0).toGps();
+    final oneMeter = (lat: 1, long: 0).toGps();
     expect(subsystems.throttle, 0);
     expect(subsystems.left, 0);
     expect(subsystems.right, 0);
diff --git a/test/orchestrator_test.dart b/test/orchestrator_test.dart
index 27549f5..7144f89 100644
--- a/test/orchestrator_test.dart
+++ b/test/orchestrator_test.dart
@@ -13,9 +13,9 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () {
     await simulator.init();
     simulator.pathfinder = RoverPathfinder(collection: simulator);
     simulator.orchestrator = RoverOrchestrator(collection: simulator);
-    simulator.pathfinder.recordObstacle((2, 0).toGps());
+    simulator.pathfinder.recordObstacle((lat: 2, long: 0).toGps());
     // Test blocked command:
-    final command = AutonomyCommand(destination: (2, 0).toGps(), task: AutonomyTask.GPS_ONLY);
+    final command = AutonomyCommand(destination: (lat: 2, long: 0).toGps(), task: AutonomyTask.GPS_ONLY);
     expect(simulator.gps.latitude, 0);
     expect(simulator.gps.longitude, 0);
     expect(simulator.imu.heading, 0);
@@ -37,13 +37,13 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () {
     simulator.pathfinder = RoverPathfinder(collection: simulator);
     simulator.orchestrator = RoverOrchestrator(collection: simulator);
     simulator.pathfinder.obstacles.addAll([
-      (2, 0).toGps(),
-      (4, -1).toGps(),
-      (4, 1).toGps(),
+      (lat: 2, long: 0).toGps(),
+      (lat: 4, long: -1).toGps(),
+      (lat: 4, long: 1).toGps(),
     ]);
     await simulator.init();
     // Test normal command:
-    final destination = (4, 0).toGps();
+    final destination = (lat: 4, long: 0).toGps();
     final command = AutonomyCommand(destination: destination, task: AutonomyTask.GPS_ONLY);
     expect(simulator.gps.latitude, 0);
     expect(simulator.gps.longitude, 0);
@@ -56,9 +56,9 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () {
     expect(status1.task, AutonomyTask.AUTONOMY_TASK_UNDEFINED);
     expect(status1.destination, GpsCoordinates());
     expect(status1.obstacles, [
-      (2, 0).toGps(),
-      (4, -1).toGps(),
-      (4, 1).toGps(),
+      (lat: 2, long: 0).toGps(),
+      (lat: 4, long: -1).toGps(),
+      (lat: 4, long: 1).toGps(),
     ]);
     expect(status1.state, AutonomyState.AT_DESTINATION);
     await simulator.dispose();
@@ -68,9 +68,9 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () {
     Logger.level = LogLevel.off;
     final simulator = AutonomySimulator();
     final obstacles = <SimulatedObstacle>[
-      SimulatedObstacle(coordinates: (2, 0).toGps(), radius: 1),
-      SimulatedObstacle(coordinates: (6, -1).toGps(), radius: 1),
-      SimulatedObstacle(coordinates: (6, 1).toGps(), radius: 1),
+      SimulatedObstacle(coordinates: (lat: 2, long: 0).toGps(), radius: 1),
+      SimulatedObstacle(coordinates: (lat: 6, long: -1).toGps(), radius: 1),
+      SimulatedObstacle(coordinates: (lat: 6, long: 1).toGps(), radius: 1),
     ];
     simulator.detector = DetectorSimulator(collection: simulator, obstacles: obstacles);
     simulator.pathfinder = RoverPathfinder(collection: simulator);
@@ -78,7 +78,7 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () {
     simulator.drive = DriveSimulator(collection: simulator);
     await simulator.init();
     final origin = GpsCoordinates();
-    final destination = (0, 7).toGps();
+    final destination = (lat: 0, long: 7).toGps();
     expect(simulator.gps.isNear(origin), isTrue);
     expect(simulator.imu.heading, 0);
     final command = AutonomyCommand(destination: destination, task: AutonomyTask.GPS_ONLY);
@@ -89,8 +89,8 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () {
 
   test("Rejects commands until latitude has been determined", () async {
     final simulator = AutonomySimulator();
-    final start = (5, 0).toGps();
-    final destination = (5, 5).toGps();
+    final start = (lat: 5, long: 0).toGps();
+    final destination = (lat: 5, long: 5).toGps();
     final command = AutonomyCommand(destination: destination, task: AutonomyTask.GPS_ONLY);
     simulator.orchestrator = RoverOrchestrator(collection: simulator);
     simulator.pathfinder = RoverPathfinder(collection: simulator);
diff --git a/test/path_test.dart b/test/path_test.dart
index 499fe46..9f43d99 100644
--- a/test/path_test.dart
+++ b/test/path_test.dart
@@ -11,7 +11,7 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
 
   test("Simple path from (0, 0) to (5, 5) exists", () {
     final simulator = AutonomySimulator();
-    final destination = (5, 5).toGps();
+    final destination = (lat: 5, long: 5).toGps();
     simulator.logger.info("Each step is ${GpsUtils.north.latitude.toStringAsFixed(5)}");
     simulator.logger.info("Going to ${destination.prettyPrint()}");
     simulator.pathfinder = RoverPathfinder(collection: simulator);  
@@ -26,7 +26,7 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
 
     // Plan a path from (0, 0) to (5, 5)
     simulator.pathfinder = RoverPathfinder(collection: simulator);  
-    final destination = (5, 5).toGps();
+    final destination = (lat: 5, long: 5).toGps();
     simulator.logger.info("Going to ${destination.prettyPrint()}");
     final path = simulator.pathfinder.getPath(destination);
     expect(path, isNotNull); if (path == null) return;
@@ -49,7 +49,7 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
   
   test("Following path gets to the end", () async { 
     final simulator = AutonomySimulator();
-    final destination = (5, 5).toGps();
+    final destination = (lat: 5, long: 5).toGps();
     simulator.pathfinder = RoverPathfinder(collection: simulator);
     final path = simulator.pathfinder.getPath(destination);
     
@@ -65,10 +65,10 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
   test("Avoid obstacles but reach the goal", () async {
     // Logger.level = LogLevel.all;
     final simulator = AutonomySimulator();
-    final destination = (5, 0).toGps();
+    final destination = (lat: 5, long: 0).toGps();
     simulator.pathfinder = RoverPathfinder(collection: simulator);
-    simulator.pathfinder.recordObstacle((1, 0).toGps());
-    simulator.pathfinder.recordObstacle((2, 0).toGps());
+    simulator.pathfinder.recordObstacle((lat: 1, long: 0).toGps());
+    simulator.pathfinder.recordObstacle((lat: 2, long: 0).toGps());
     final path = simulator.pathfinder.getPath(destination);
     expect(path, isNotNull);
     if (path == null) {
@@ -92,7 +92,7 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
     simulator.pathfinder = RoverPathfinder(collection: simulator);
     simulator.logger.trace("Starting from ${simulator.gps.coordinates.prettyPrint()}");
     simulator.logger.trace("Each step is +/- ${GpsUtils.north.prettyPrint()}");
-    final destination = (1000, 1000).toGps();
+    final destination = (lat: 1000, long: 1000).toGps();
     simulator.logger.info("Going to ${destination.prettyPrint()}");
     final path = simulator.pathfinder.getPath(destination);
     expect(path, isNotNull);
@@ -103,11 +103,11 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
   test("Impossible paths are reported", () async {
     final simulator = AutonomySimulator();
     simulator.pathfinder = RoverPathfinder(collection: simulator);
-    final destination = (5, 5).toGps();
+    final destination = (lat: 5, long: 5).toGps();
     final obstacles = {
-      (1, -1).toGps(),  (1, 0).toGps(),  (1, 1).toGps(),
-      (0, -1).toGps(),   /* Rover */     (0, 1).toGps(),
-      (-1, -1).toGps(), (-1, 0).toGps(), (-1, 1).toGps(),
+      (lat: 1, long: -1).toGps(),  (lat: 1, long: 0).toGps(),  (lat: 1, long: 1).toGps(),
+      (lat: 0, long: -1).toGps(),          /* Rover */         (lat: 0, long: 1).toGps(),
+      (lat: -1, long: -1).toGps(), (lat: -1, long: 0).toGps(), (lat: -1, long: 1).toGps(),
     };
     for (final obstacle in obstacles) {
       simulator.pathfinder.recordObstacle(obstacle);
diff --git a/test/rover_test.dart b/test/rover_test.dart
index 2f40af5..2fb4bbb 100644
--- a/test/rover_test.dart
+++ b/test/rover_test.dart
@@ -21,7 +21,7 @@ void main() => group("[Rover]", tags: ["rover"], () {
     final simulator = AutonomySimulator();
     simulator.pathfinder = RoverPathfinder(collection: simulator);
     await testPath(simulator);
-    simulator.gps.update((0, 0).toGps());
+    simulator.gps.update((lat: 0, long: 0).toGps());
     simulator.imu.update(Orientation());
     await testPath2(simulator);
     await simulator.dispose();
@@ -29,7 +29,7 @@ void main() => group("[Rover]", tags: ["rover"], () {
 
   test("Waits for sensor data", () async {
     final rover = RoverAutonomy();
-    final position = (5, 5).toGps();
+    final position = (lat: 5, long: 5).toGps();
     final orientation = Orientation();
     final data = VideoData();
 
@@ -58,7 +58,7 @@ void main() => group("[Rover]", tags: ["rover"], () {
 });
 
 Future<void> testPath(AutonomyInterface simulator) async {
-  final destination = (5, 5).toGps();
+  final destination = (lat: 5, long: 5).toGps();
   final result = simulator.pathfinder.getPath(destination);
   expect(simulator.gps.latitude, 0);
   expect(simulator.gps.longitude, 0);
@@ -78,10 +78,10 @@ Future<void> testPath(AutonomyInterface simulator) async {
 
 Future<void> testPath2(AutonomyInterface simulator) async {
   // Logger.level = LogLevel.all;
-  final destination = (4, 0).toGps();
-  simulator.pathfinder.recordObstacle((2, 0).toGps());
-  simulator.pathfinder.recordObstacle((4, -1).toGps());
-  simulator.pathfinder.recordObstacle((4, 1).toGps());
+  final destination = (lat: 4, long: 0).toGps();
+  simulator.pathfinder.recordObstacle((lat: 2, long: 0).toGps());
+  simulator.pathfinder.recordObstacle((lat: 4, long: -1).toGps());
+  simulator.pathfinder.recordObstacle((lat: 4, long: 1).toGps());
   final result = simulator.pathfinder.getPath(destination);
   expect(simulator.gps.latitude, 0);
   expect(simulator.gps.longitude, 0);
diff --git a/test/sensor_test.dart b/test/sensor_test.dart
index 623e2cb..017662a 100644
--- a/test/sensor_test.dart
+++ b/test/sensor_test.dart
@@ -16,13 +16,13 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
   tearDown(() => Logger.level = LogLevel.off);
 
   test("GpsUtils.isNear", () {
-    final origin = (0, 0).toGps();
+    final origin = (lat: 0, long: 0).toGps();
     expect(GpsCoordinates(latitude: 0, longitude: 0), origin);
     expect(origin.isNear(origin), isTrue);
 
-    final a = (5, 5).toGps();
-    final a2 = (5, 5).toGps();
-    final b = (5, 6.5).toGps();
+    final a = (lat: 5, long: 5).toGps();
+    final a2 = (lat: 5, long: 5).toGps();
+    final b = (lat: 5, long: 6.5).toGps();
 
     expect(a.isNear(a), isTrue);
     expect(a.isNear(a2), isTrue);
@@ -36,8 +36,8 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     expect(c.isNear(e), isFalse);
     expect(d.isNear(e), isFalse);
 
-    final f = (12, 12).toGps();
-    final g = (12.2, 12.2).toGps();
+    final f = (lat: 12, long: 12).toGps();
+    final g = (lat: 12.2, long: 12.2).toGps();
     expect(f.isNear(g), isTrue);
   });
 

From e480c377b6406325490b45cb9ed3a0040cf668f7 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Thu, 28 Nov 2024 15:21:52 -0500
Subject: [PATCH 09/21] Fixed toGps() in task.dart

---
 bin/task.dart | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/bin/task.dart b/bin/task.dart
index 798289b..e51da7b 100644
--- a/bin/task.dart
+++ b/bin/task.dart
@@ -3,10 +3,10 @@ import "package:autonomy/simulator.dart";
 import "package:autonomy/rover.dart";
 import "package:burt_network/logging.dart";
 
-final chair = (2, 0).toGps();
+final chair = (lat: 2, long: 0).toGps();
 final obstacles = <SimulatedObstacle>[
-  SimulatedObstacle(coordinates: (6, -1).toGps(), radius: 3),
-  SimulatedObstacle(coordinates: (6, 1).toGps(), radius: 3),
+  SimulatedObstacle(coordinates: (lat: 6, long: -1).toGps(), radius: 3),
+  SimulatedObstacle(coordinates: (lat: 6, long: 1).toGps(), radius: 3),
 ];
 // Enter in the Dashboard: Destination = (lat=7, long=0);
 

From a41b18441cf14f846ed5a86935a0c36741d6a507 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Thu, 28 Nov 2024 15:44:08 -0500
Subject: [PATCH 10/21] Specified networking library version

---
 pubspec.lock | 8 ++++----
 pubspec.yaml | 4 +++-
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/pubspec.lock b/pubspec.lock
index 7366b66..69567f5 100644
--- a/pubspec.lock
+++ b/pubspec.lock
@@ -58,11 +58,11 @@ packages:
     dependency: "direct main"
     description:
       path: "."
-      ref: HEAD
-      resolved-ref: ceeb9602a332d612bd9392d2b865403eea76b465
-      url: "https://github.com/BinghamtonRover/Dart-Networking.git"
+      ref: "2.2.0"
+      resolved-ref: "7ec80052bd9d2777782f7ce44cfcfc1e69e8a582"
+      url: "https://github.com/BinghamtonRover/Networking.git"
     source: git
-    version: "2.0.0"
+    version: "2.2.0"
   collection:
     dependency: transitive
     description:
diff --git a/pubspec.yaml b/pubspec.yaml
index c117b53..daf64d3 100644
--- a/pubspec.yaml
+++ b/pubspec.yaml
@@ -11,7 +11,9 @@ dependencies:
   opencv_ffi:
     git: https://github.com/BinghamtonRover/OpenCV-FFI.git
   burt_network:
-    git: https://github.com/BinghamtonRover/Dart-Networking.git
+    git: 
+      url: https://github.com/BinghamtonRover/Networking.git
+      ref: 2.2.0
   a_star: ^3.0.0
   meta: ^1.11.0
 

From e19dae721d1c10e38965deb17006ff83bdc18728 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Thu, 28 Nov 2024 16:14:23 -0500
Subject: [PATCH 11/21] Fixed isNear method

---
 lib/src/interfaces/gps_utils.dart | 13 +++++++++++--
 1 file changed, 11 insertions(+), 2 deletions(-)

diff --git a/lib/src/interfaces/gps_utils.dart b/lib/src/interfaces/gps_utils.dart
index 83d4711..dc6c19e 100644
--- a/lib/src/interfaces/gps_utils.dart
+++ b/lib/src/interfaces/gps_utils.dart
@@ -42,8 +42,17 @@ extension GpsUtils on GpsCoordinates {
 
   bool isNear(GpsCoordinates other, [double? tolerance]) {
     tolerance ??= maxErrorMeters;
-    return (latitude - other.latitude).abs() < tolerance * latitudePerMeter &&
-        (longitude - other.longitude).abs() < tolerance * longitudePerMeter;
+    final currentMeters = inMeters;
+    final otherMeters = other.inMeters;
+
+    final (deltaX, deltaY) = (
+      currentMeters.lat - otherMeters.lat,
+      currentMeters.long - otherMeters.long
+    );
+
+    final distance = sqrt(pow(deltaX, 2) + pow(deltaY, 2));
+
+    return distance < tolerance;
   }
 
   GpsCoordinates operator +(GpsCoordinates other) => GpsCoordinates(

From c0d71ae941a8dc90eb766c5728e7ec214d34a886 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Sun, 8 Dec 2024 03:57:58 -0500
Subject: [PATCH 12/21] Refactor and Reorganization (#7)

Co-authored-by: Binghamton Rover <rover@binghamton.edu>
Co-authored-by: Levi Lesches <levilesches@gmail.com>
---
 .vscode/settings.json                         |   5 +
 analysis_options.yaml                         |  17 +-
 bin/arcuo.dart                                |   2 +-
 bin/path.dart                                 |   2 +-
 bin/random.dart                               |   3 +-
 bin/sensorless.dart                           | 115 +++++----
 bin/tank_autonomy.dart                        |  25 --
 bin/task.dart                                 |   5 +-
 bin/test.dart                                 |  42 ++--
 lib/interfaces.dart                           |  29 +--
 lib/rover.dart                                |  29 ++-
 lib/simulator.dart                            |  21 +-
 .../autonomy.dart => autonomy_interface.dart} |  13 +-
 .../detector_interface.dart}                  |   0
 .../network_detector.dart                     |  10 +-
 .../rover_detector.dart}                      |   2 +-
 .../sim_detector.dart}                        |   7 +-
 lib/src/drive/drive_commands.dart             |  39 ++++
 lib/src/drive/drive_config.dart               |  40 ++++
 lib/src/drive/drive_interface.dart            |  55 +++++
 lib/src/drive/rover_drive.dart                |  88 +++++++
 lib/src/drive/sensor_drive.dart               |  87 +++++++
 lib/src/drive/sim_drive.dart                  |  29 +++
 lib/src/drive/timed_drive.dart                |  79 +++++++
 .../gps.dart => gps/gps_interface.dart}       |   1 -
 lib/src/gps/rover_gps.dart                    |  42 ++++
 .../{simulator/gps.dart => gps/sim_gps.dart}  |   1 -
 lib/src/imu/cardinal_direction.dart           |  79 +++++++
 .../imu.dart => imu/imu_interface.dart}       |  10 +-
 lib/src/imu/rover_imu.dart                    |  43 ++++
 .../{simulator/imu.dart => imu/sim_imu.dart}  |   9 +-
 lib/src/interfaces/a_star.dart                | 167 -------------
 lib/src/interfaces/drive.dart                 | 125 ----------
 lib/src/interfaces/imu_utils.dart             |  28 ---
 lib/src/interfaces/receiver.dart              |   9 -
 lib/src/interfaces/server.dart                |  25 --
 lib/src/interfaces/service.dart               |   1 -
 .../orchestrator_interface.dart}              |  12 +-
 .../rover_orchestrator.dart}                  |   8 +-
 .../sim_orchestrator.dart}                    |   1 -
 .../pathfinding_interface.dart}               |   1 -
 .../rover_pathfinding.dart}                   |   6 +-
 lib/src/pathfinding/sim_pathfinding.dart      |  24 ++
 lib/src/rover/drive.dart                      |  73 ------
 lib/src/rover/drive/motors.dart               |  26 ---
 lib/src/rover/drive/sensor.dart               | 219 ------------------
 lib/src/rover/drive/timed.dart                |  86 -------
 lib/src/rover/gps.dart                        |  41 ----
 lib/src/rover/imu.dart                        |  44 ----
 lib/src/rover/sensorless.dart                 |  76 ------
 lib/src/simulator/drive.dart                  |  73 ------
 lib/src/simulator/pathfinding.dart            |  25 --
 lib/src/utils/a_star.dart                     | 184 +++++++++++++++
 lib/src/{rover => utils}/corrector.dart       |   9 +-
 lib/src/{interfaces => utils}/error.dart      |   0
 lib/src/{interfaces => utils}/gps_utils.dart  |  35 ++-
 lib/src/utils/imu_utils.dart                  |  29 +++
 lib/src/utils/receiver.dart                   |  21 ++
 lib/src/{interfaces => utils}/reporter.dart   |   3 +-
 .../video.dart => video/rover_video.dart}     |  11 +-
 .../realsense.dart => video/sim_video.dart}   |   5 +-
 .../video.dart => video/video_interface.dart} |   5 +-
 lib/utils.dart                                |   9 +
 pubspec.lock                                  |  19 +-
 pubspec.yaml                                  |   6 +-
 test/network_test.dart                        |   3 -
 test/orchestrator_test.dart                   |   1 -
 test/path_test.dart                           |   2 +-
 test/rover_test.dart                          |  16 +-
 test/sensor_test.dart                         |  18 +-
 70 files changed, 1065 insertions(+), 1310 deletions(-)
 create mode 100644 .vscode/settings.json
 delete mode 100644 bin/tank_autonomy.dart
 rename lib/src/{interfaces/autonomy.dart => autonomy_interface.dart} (85%)
 rename lib/src/{interfaces/detector.dart => detector/detector_interface.dart} (100%)
 rename lib/src/{simulator => detector}/network_detector.dart (79%)
 rename lib/src/{rover/detector.dart => detector/rover_detector.dart} (89%)
 rename lib/src/{simulator/detector.dart => detector/sim_detector.dart} (88%)
 create mode 100644 lib/src/drive/drive_commands.dart
 create mode 100644 lib/src/drive/drive_config.dart
 create mode 100644 lib/src/drive/drive_interface.dart
 create mode 100644 lib/src/drive/rover_drive.dart
 create mode 100644 lib/src/drive/sensor_drive.dart
 create mode 100644 lib/src/drive/sim_drive.dart
 create mode 100644 lib/src/drive/timed_drive.dart
 rename lib/src/{interfaces/gps.dart => gps/gps_interface.dart} (93%)
 create mode 100644 lib/src/gps/rover_gps.dart
 rename lib/src/{simulator/gps.dart => gps/sim_gps.dart} (93%)
 create mode 100644 lib/src/imu/cardinal_direction.dart
 rename lib/src/{interfaces/imu.dart => imu/imu_interface.dart} (57%)
 create mode 100644 lib/src/imu/rover_imu.dart
 rename lib/src/{simulator/imu.dart => imu/sim_imu.dart} (77%)
 delete mode 100644 lib/src/interfaces/a_star.dart
 delete mode 100644 lib/src/interfaces/drive.dart
 delete mode 100644 lib/src/interfaces/imu_utils.dart
 delete mode 100644 lib/src/interfaces/receiver.dart
 delete mode 100644 lib/src/interfaces/server.dart
 delete mode 100644 lib/src/interfaces/service.dart
 rename lib/src/{interfaces/orchestrator.dart => orchestrator/orchestrator_interface.dart} (83%)
 rename lib/src/{rover/orchestrator.dart => orchestrator/rover_orchestrator.dart} (93%)
 rename lib/src/{simulator/orchestrator.dart => orchestrator/sim_orchestrator.dart} (92%)
 rename lib/src/{interfaces/pathfinding.dart => pathfinding/pathfinding_interface.dart} (92%)
 rename lib/src/{rover/pathfinding.dart => pathfinding/rover_pathfinding.dart} (73%)
 create mode 100644 lib/src/pathfinding/sim_pathfinding.dart
 delete mode 100644 lib/src/rover/drive.dart
 delete mode 100644 lib/src/rover/drive/motors.dart
 delete mode 100644 lib/src/rover/drive/sensor.dart
 delete mode 100644 lib/src/rover/drive/timed.dart
 delete mode 100644 lib/src/rover/gps.dart
 delete mode 100644 lib/src/rover/imu.dart
 delete mode 100644 lib/src/rover/sensorless.dart
 delete mode 100644 lib/src/simulator/drive.dart
 delete mode 100644 lib/src/simulator/pathfinding.dart
 create mode 100644 lib/src/utils/a_star.dart
 rename lib/src/{rover => utils}/corrector.dart (88%)
 rename lib/src/{interfaces => utils}/error.dart (100%)
 rename lib/src/{interfaces => utils}/gps_utils.dart (72%)
 create mode 100644 lib/src/utils/imu_utils.dart
 create mode 100644 lib/src/utils/receiver.dart
 rename lib/src/{interfaces => utils}/reporter.dart (86%)
 rename lib/src/{rover/video.dart => video/rover_video.dart} (67%)
 rename lib/src/{simulator/realsense.dart => video/sim_video.dart} (89%)
 rename lib/src/{interfaces/video.dart => video/video_interface.dart} (71%)
 create mode 100644 lib/utils.dart

diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..14029d2
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,5 @@
+{
+  "cSpell.words": [
+    "setpoint"
+  ]
+}
\ No newline at end of file
diff --git a/analysis_options.yaml b/analysis_options.yaml
index 76649d1..0f5506c 100644
--- a/analysis_options.yaml
+++ b/analysis_options.yaml
@@ -9,23 +9,24 @@ include: package:very_good_analysis/analysis_options.yaml  # has more lints
 
 analyzer:
   language:
-    # Strict casts isn't helpful with null safety. It only notifies you on `dynamic`, 
-    # which happens all the time in JSON. 
-    # 
+    # Strict casts isn't helpful with null safety. It only notifies you on `dynamic`,
+    # which happens all the time in JSON.
+    #
     # See https://github.com/dart-lang/language/blob/main/resources/type-system/strict-casts.md
     strict-casts: false
 
     # Don't let any types be inferred as `dynamic`.
-    # 
+    #
     # See https://github.com/dart-lang/language/blob/main/resources/type-system/strict-inference.md
     strict-inference: true
 
-    # Don't let Dart infer the wrong type on the left side of an assignment. 
-    # 
+    # Don't let Dart infer the wrong type on the left side of an assignment.
+    #
     # See https://github.com/dart-lang/language/blob/main/resources/type-system/strict-raw-types.md
     strict-raw-types: true
 
-  exclude: 
+  exclude:
+    - test/*.dart
 
 linter:
   rules:
@@ -44,7 +45,7 @@ linter:
     sort_constructors_first: false  # final properties, then constructor
     avoid_dynamic_calls: false  # this lint takes over errors in the IDE
     cascade_invocations: false  # cascades are often less readable
-    
+
     # Temporarily disabled lints
     public_member_api_docs: false  # until we are ready to document
     flutter_style_todos: false  # until we are ready to address them
diff --git a/bin/arcuo.dart b/bin/arcuo.dart
index ae9f7d8..d924090 100644
--- a/bin/arcuo.dart
+++ b/bin/arcuo.dart
@@ -2,7 +2,7 @@ import "package:autonomy/autonomy.dart";
 
 void main() async {
   final rover = RoverAutonomy();
-	rover.drive = RoverDrive(collection: rover, useImu: false, useGps: false);
+  rover.drive = RoverDrive(collection: rover, useImu: false, useGps: false);
   await rover.init();
   //await rover.waitForValue();
   final didSeeAruco = await rover.drive.spinForAruco();
diff --git a/bin/path.dart b/bin/path.dart
index 58fe8ba..8ae1f28 100644
--- a/bin/path.dart
+++ b/bin/path.dart
@@ -18,7 +18,7 @@ void main() {
   }
   var turnCount = 0;
   for (final step in path) {
-    if (step.direction == DriveDirection.left || step.direction == DriveDirection.right) {
+    if (step.instruction == DriveDirection.left || step.instruction == DriveDirection.right) {
       turnCount++;
     }
   }
diff --git a/bin/random.dart b/bin/random.dart
index fe52f37..c356d40 100644
--- a/bin/random.dart
+++ b/bin/random.dart
@@ -1,7 +1,6 @@
 // ignore_for_file: avoid_print
 
 import "package:autonomy/interfaces.dart";
-import "package:autonomy/src/rover/corrector.dart";
 
 const maxError = GpsInterface.gpsError;
 const maxSamples = 10;
@@ -17,7 +16,7 @@ bool test() {
     corrector.addValue(value);
     if (verbose) {
       final calibrated = corrector.calibratedValue;
-      print("Current value: $value, Corrected value: $calibrated");    
+      print("Current value: $value, Corrected value: $calibrated");
       print("  Difference: ${calibrated.toStringAsFixed(7)} < ${epsilon.toStringAsFixed(7)}");
     }
   }
diff --git a/bin/sensorless.dart b/bin/sensorless.dart
index acc2569..1605430 100644
--- a/bin/sensorless.dart
+++ b/bin/sensorless.dart
@@ -1,64 +1,63 @@
-import "package:burt_network/logging.dart";
-import "package:burt_network/generated.dart";
+// import "package:burt_network/logging.dart";
+// import "package:burt_network/protobuf.dart";
 
-import "package:autonomy/interfaces.dart";
-import "package:autonomy/simulator.dart";
-import "package:autonomy/rover.dart";
+// import "package:autonomy/simulator.dart";
+// import "package:autonomy/rover.dart";
 
-void main() async {
-  Logger.level = LogLevel.debug;
-  final simulator = AutonomySimulator();
-  simulator.drive = SensorlessDrive(collection: simulator, useGps: false, useImu: false);
-  await simulator.init();
-  await simulator.server.waitForConnection();
+// void main() async {
+//   Logger.level = LogLevel.debug;
+//   final simulator = AutonomySimulator();
+//   simulator.drive = SensorlessDrive(collection: simulator, useGps: false, useImu: false);
+//   await simulator.init();
+//   await simulator.server.waitForConnection();
 
-  final message = AutonomyData(
-    destination: GpsCoordinates(latitude: 0, longitude: 0),
-    state: AutonomyState.DRIVING,
-    task: AutonomyTask.GPS_ONLY,
-    obstacles: [],
-    path: [
-      for (double x = 0; x < 3; x++)
-        for (double y = 0; y < 3; y++)
-          GpsCoordinates(latitude: y, longitude: x),
-    ],
-  );
-  simulator.server.sendMessage(message);
+//   final message = AutonomyData(
+//     destination: GpsCoordinates(latitude: 0, longitude: 0),
+//     state: AutonomyState.DRIVING,
+//     task: AutonomyTask.GPS_ONLY,
+//     obstacles: [],
+//     path: [
+//       for (double x = 0; x < 3; x++)
+//         for (double y = 0; y < 3; y++)
+//           GpsCoordinates(latitude: y, longitude: x),
+//     ],
+//   );
+//   simulator.server.sendMessage(message);
 
-  // "Snakes" around a 3x3 meter box.    (0, 0), North
-  await simulator.drive.goForward();  // (1, 0), North
-  await simulator.drive.goForward();  // (2, 0), North
-  await simulator.drive.turnRight();  // (2, 0), East
-  await simulator.drive.goForward();  // (2, 1), East
-  await simulator.drive.turnRight();  // (2, 1), South
-  await simulator.drive.goForward();  // (1, 1), South
-  await simulator.drive.goForward();  // (0, 1), South
-  await simulator.drive.turnLeft();   // (0, 1), East
-  await simulator.drive.goForward();  // (0, 2), East
-  await simulator.drive.turnLeft();   // (0, 2), North
-  await simulator.drive.goForward();  // (1, 2), North
-  await simulator.drive.goForward();  // (2, 2), North
-  await simulator.drive.turnLeft();   // (2, 2), West
-  await simulator.drive.goForward();  // (2, 1), West
-  await simulator.drive.goForward();  // (2, 0), West
-  await simulator.drive.turnLeft();   // (2, 0), South
-  await simulator.drive.goForward();  // (1, 0), South
-  await simulator.drive.goForward();  // (0, 0), South
-  await simulator.drive.turnLeft();   // (0, 0), East
-  await simulator.drive.turnLeft();   // (0, 0), North
+//   // "Snakes" around a 3x3 meter box.    (0, 0), North
+//   await simulator.drive.goForward();  // (1, 0), North
+//   await simulator.drive.goForward();  // (2, 0), North
+//   await simulator.drive.turnRight();  // (2, 0), East
+//   await simulator.drive.goForward();  // (2, 1), East
+//   await simulator.drive.turnRight();  // (2, 1), South
+//   await simulator.drive.goForward();  // (1, 1), South
+//   await simulator.drive.goForward();  // (0, 1), South
+//   await simulator.drive.turnLeft();   // (0, 1), East
+//   await simulator.drive.goForward();  // (0, 2), East
+//   await simulator.drive.turnLeft();   // (0, 2), North
+//   await simulator.drive.goForward();  // (1, 2), North
+//   await simulator.drive.goForward();  // (2, 2), North
+//   await simulator.drive.turnLeft();   // (2, 2), West
+//   await simulator.drive.goForward();  // (2, 1), West
+//   await simulator.drive.goForward();  // (2, 0), West
+//   await simulator.drive.turnLeft();   // (2, 0), South
+//   await simulator.drive.goForward();  // (1, 0), South
+//   await simulator.drive.goForward();  // (0, 0), South
+//   await simulator.drive.turnLeft();   // (0, 0), East
+//   await simulator.drive.turnLeft();   // (0, 0), North
 
-  final message2 = AutonomyData(
-    state: AutonomyState.AT_DESTINATION,
-    task: AutonomyTask.AUTONOMY_TASK_UNDEFINED,
-    obstacles: [],
-    path: [
-      for (double x = 0; x < 3; x++)
-        for (double y = 0; y < 3; y++)
-          GpsCoordinates(latitude: y, longitude: x),
-    ],
-  );
-  simulator.server.sendMessage(message2);
-  simulator.server.sendMessage(message2);
+//   final message2 = AutonomyData(
+//     state: AutonomyState.AT_DESTINATION,
+//     task: AutonomyTask.AUTONOMY_TASK_UNDEFINED,
+//     obstacles: [],
+//     path: [
+//       for (double x = 0; x < 3; x++)
+//         for (double y = 0; y < 3; y++)
+//           GpsCoordinates(latitude: y, longitude: x),
+//     ],
+//   );
+//   simulator.server.sendMessage(message2);
+//   simulator.server.sendMessage(message2);
 
-  await simulator.dispose();
-}
+//   await simulator.dispose();
+// }
diff --git a/bin/tank_autonomy.dart b/bin/tank_autonomy.dart
deleted file mode 100644
index dac6948..0000000
--- a/bin/tank_autonomy.dart
+++ /dev/null
@@ -1,25 +0,0 @@
-import "dart:io";
-
-import "package:autonomy/interfaces.dart";
-import "package:autonomy/rover.dart";
-import "package:autonomy/simulator.dart";
-import "package:autonomy/src/rover/imu.dart";
-import "package:autonomy/src/simulator/network_detector.dart";
-import "package:burt_network/burt_network.dart";
-
-void main() async {
-  ServerUtils.subsystemsDestination = SocketInfo(
-    address: InternetAddress("192.168.1.40"),
-    port: 8001,
-  );
-  final tank = RoverAutonomy();
-  tank.detector = NetworkDetector(collection: tank);
-  tank.gps = GpsSimulator(collection: tank);
-  // tank.imu = ImuSimulator(collection: tank);
-  tank.imu = RoverImu(collection: tank);
-  tank.drive = RoverDrive(collection: tank, useGps: false);
-  await tank.init();
-  await tank.imu.waitForValue();
-
-  await tank.server.waitForConnection();
-}
diff --git a/bin/task.dart b/bin/task.dart
index e51da7b..628f823 100644
--- a/bin/task.dart
+++ b/bin/task.dart
@@ -1,7 +1,6 @@
-import "package:autonomy/interfaces.dart";
 import "package:autonomy/simulator.dart";
 import "package:autonomy/rover.dart";
-import "package:burt_network/logging.dart";
+import "package:burt_network/burt_network.dart";
 
 final chair = (lat: 2, long: 0).toGps();
 final obstacles = <SimulatedObstacle>[
@@ -22,8 +21,6 @@ void main() async {
   simulator.drive = DriveSimulator(collection: simulator, shouldDelay: true);
   await simulator.init();
   await simulator.imu.waitForValue();
-//	await simulator.drive.faceNorth();
   await simulator.server.waitForConnection();
-	print("Ready");
 
 }
diff --git a/bin/test.dart b/bin/test.dart
index acae8bd..63fa627 100644
--- a/bin/test.dart
+++ b/bin/test.dart
@@ -1,24 +1,22 @@
-import "package:autonomy/autonomy.dart";
-import "package:burt_network/logging.dart";
-import "package:autonomy/src/rover/gps.dart";
-import "package:autonomy/src/rover/imu.dart";
+// import "package:autonomy/autonomy.dart";
+// import "package:burt_network/logging.dart";
 
-void main() async {
-  Logger.level = LogLevel.all;
-  final rover = RoverAutonomy();
-  rover.gps = RoverGps(collection: rover);
-  rover.imu = RoverImu(collection: rover);
-  rover.drive = RoverDrive(collection: rover, useGps: false, useImu: true);
-  
-  await rover.init();
-  print("Waiting for readings");
-//  await rover.waitForReadings();
-//  await rover.waitForConnection();
+// void main() async {
+//   Logger.level = LogLevel.all;
+//   final rover = RoverAutonomy();
+//   rover.gps = RoverGps(collection: rover);
+//   rover.imu = RoverImu(collection: rover);
+//   rover.drive = RoverDrive(collection: rover, useGps: false);
 
-  rover.logger.info("Starting");
-  await rover.drive.turnLeft();
-await rover.drive.turnRight();
- 
-  rover.logger.info("Done");
-  await rover.dispose();
-}
+//   await rover.init();
+//   rover.logger.info("Waiting for readings");
+// //  await rover.waitForReadings();
+// //  await rover.waitForConnection();
+
+//   rover.logger.info("Starting");
+//   await rover.drive.turnLeft();
+//   await rover.drive.turnRight();
+
+//   rover.logger.info("Done");
+//   await rover.dispose();
+// }
diff --git a/lib/interfaces.dart b/lib/interfaces.dart
index a6ecf3b..90b06b4 100644
--- a/lib/interfaces.dart
+++ b/lib/interfaces.dart
@@ -1,17 +1,12 @@
-export "src/interfaces/a_star.dart";
-export "src/interfaces/autonomy.dart";
-export "src/interfaces/detector.dart";
-export "src/interfaces/drive.dart";
-export "src/interfaces/error.dart";
-export "src/interfaces/gps.dart";
-export "src/interfaces/gps_utils.dart";
-export "src/interfaces/imu.dart";
-export "src/interfaces/imu_utils.dart";
-export "src/interfaces/pathfinding.dart";
-export "src/interfaces/server.dart";
-export "src/interfaces/video.dart";
-export "src/interfaces/receiver.dart";
-export "src/interfaces/reporter.dart";
-export "src/interfaces/service.dart";
-export "src/interfaces/orchestrator.dart";
-export "package:burt_network/src/utils.dart";
+export "src/autonomy_interface.dart";
+export "src/detector/detector_interface.dart";
+export "src/drive/drive_interface.dart";
+export "src/gps/gps_interface.dart";
+export "src/imu/imu_interface.dart";
+export "src/imu/cardinal_direction.dart";
+export "src/pathfinding/pathfinding_interface.dart";
+export "src/video/video_interface.dart";
+export "src/orchestrator/orchestrator_interface.dart";
+
+export "utils.dart";
+export "package:burt_network/protobuf.dart";
diff --git a/lib/rover.dart b/lib/rover.dart
index 9f664d9..38f5845 100644
--- a/lib/rover.dart
+++ b/lib/rover.dart
@@ -1,26 +1,23 @@
-export "src/rover/drive.dart";
-export "src/rover/pathfinding.dart";
-export "src/rover/orchestrator.dart";
-export "src/rover/sensorless.dart";
+export "src/drive/rover_drive.dart";
+export "src/pathfinding/rover_pathfinding.dart";
+export "src/orchestrator/rover_orchestrator.dart";
+export "src/imu/rover_imu.dart";
+export "src/gps/rover_gps.dart";
+export "src/detector/network_detector.dart";
+export "src/video/rover_video.dart";
+export "src/detector/rover_detector.dart";
 
 import "package:autonomy/interfaces.dart";
+import "package:autonomy/rover.dart";
 import "package:burt_network/burt_network.dart";
 
-import "src/rover/pathfinding.dart";
-import "src/rover/drive.dart";
-import "src/rover/gps.dart";
-import "src/rover/imu.dart";
-import "src/rover/orchestrator.dart";
-import "src/rover/video.dart";
-import "src/rover/detector.dart";
-
 /// A collection of all the different services used by the autonomy program.
 class RoverAutonomy extends AutonomyInterface {
-	/// A server to communicate with the dashboard and receive data from the subsystems.
-	// @override late final AutonomyServer server = AutonomyServer(collection: this);
+  /// A server to communicate with the dashboard and receive data from the subsystems.
+  // @override late final AutonomyServer server = AutonomyServer(collection: this);
   @override late final server = RoverSocket(port: 8003, device: Device.AUTONOMY, collection: this);
-	/// A helper class to handle driving the rover.
-	@override late DriveInterface drive = RoverDrive(collection: this);
+  /// A helper class to handle driving the rover.
+  @override late DriveInterface drive = RoverDrive(collection: this);
   @override late GpsInterface gps = RoverGps(collection: this);
   @override late ImuInterface imu = RoverImu(collection: this);
   @override late final logger = BurtLogger(socket: server);
diff --git a/lib/simulator.dart b/lib/simulator.dart
index 908a8ae..8d3d068 100644
--- a/lib/simulator.dart
+++ b/lib/simulator.dart
@@ -1,20 +1,15 @@
-export "src/simulator/detector.dart";
-export "src/simulator/drive.dart";
-export "src/simulator/gps.dart";
-export "src/simulator/imu.dart";
-export "src/simulator/realsense.dart";
+export "src/detector/sim_detector.dart";
+export "src/drive/sim_drive.dart";
+export "src/gps/sim_gps.dart";
+export "src/imu/sim_imu.dart";
+export "src/video/sim_video.dart";
+export "src/pathfinding/sim_pathfinding.dart";
+export "src/orchestrator/sim_orchestrator.dart";
 
 import "package:autonomy/interfaces.dart";
-import "package:autonomy/src/simulator/orchestrator.dart";
+import "package:autonomy/simulator.dart";
 import "package:burt_network/burt_network.dart";
 
-import "src/simulator/detector.dart";
-import "src/simulator/drive.dart";
-import "src/simulator/gps.dart";
-import "src/simulator/imu.dart";
-import "src/simulator/pathfinding.dart";
-import "src/simulator/realsense.dart";
-
 class AutonomySimulator extends AutonomyInterface {
   @override late final logger = BurtLogger(socket: server);
   @override late final server = RoverSocket(port: 8003, device: Device.AUTONOMY, collection: this);
diff --git a/lib/src/interfaces/autonomy.dart b/lib/src/autonomy_interface.dart
similarity index 85%
rename from lib/src/interfaces/autonomy.dart
rename to lib/src/autonomy_interface.dart
index bfd948f..dbafe47 100644
--- a/lib/src/interfaces/autonomy.dart
+++ b/lib/src/autonomy_interface.dart
@@ -24,7 +24,6 @@ abstract class AutonomyInterface extends Service with Receiver {
     result &= await detector.init();
     result &= await video.init();
     logger.info("Init orchestrator");
-    print("Orchestrator init 1");
     await orchestrator.init();
     logger.info("Init orchestrator done");
     if (result) {
@@ -53,15 +52,17 @@ abstract class AutonomyInterface extends Service with Receiver {
     await init();
   }
 
+  List<Receiver> get _receivers => [gps, imu, video];
+
   @override
   Future<void> waitForValue() async {
     logger.info("Waiting for readings...");
-    await gps.waitForValue();
-    await imu.waitForValue();
-    await video.waitForValue();
-    logger.info("Received GPS and IMU values");
+    for (final receiver in _receivers) {
+      await receiver.waitForValue();
+    }
+    logger.info("Received all necessary values");
   }
 
   @override
-  bool get hasValue => gps.hasValue && imu.hasValue && video.hasValue;
+  bool get hasValue => _receivers.every((r) => r.hasValue);
 }
diff --git a/lib/src/interfaces/detector.dart b/lib/src/detector/detector_interface.dart
similarity index 100%
rename from lib/src/interfaces/detector.dart
rename to lib/src/detector/detector_interface.dart
diff --git a/lib/src/simulator/network_detector.dart b/lib/src/detector/network_detector.dart
similarity index 79%
rename from lib/src/simulator/network_detector.dart
rename to lib/src/detector/network_detector.dart
index 804519f..c91934d 100644
--- a/lib/src/simulator/network_detector.dart
+++ b/lib/src/detector/network_detector.dart
@@ -1,5 +1,4 @@
-import "package:autonomy/autonomy.dart";
-import "package:burt_network/burt_network.dart";
+import "package:autonomy/autonomy.dart";
 
 class NetworkDetector extends DetectorInterface {
   final List<GpsCoordinates> _newObstacles = [];
@@ -29,9 +28,10 @@ class NetworkDetector extends DetectorInterface {
   @override
   Future<bool> init() async {
     collection.server.messages.onMessage(
-        name: AutonomyData().messageName,
-        constructor: AutonomyData.fromBuffer,
-        callback: _onDataReceived);
+      name: AutonomyData().messageName,
+      constructor: AutonomyData.fromBuffer,
+      callback: _onDataReceived,
+    );
     return true;
   }
 
diff --git a/lib/src/rover/detector.dart b/lib/src/detector/rover_detector.dart
similarity index 89%
rename from lib/src/rover/detector.dart
rename to lib/src/detector/rover_detector.dart
index f28c508..3ed0161 100644
--- a/lib/src/rover/detector.dart
+++ b/lib/src/detector/rover_detector.dart
@@ -11,7 +11,7 @@ class RoverDetector extends DetectorInterface {
 
   @override
 //  bool canSeeAruco() => collection.video.data.arucoDetected == BoolState.YES;
-	bool canSeeAruco() => collection.video.flag;
+  bool canSeeAruco() => collection.video.flag;
 
   @override
   Future<bool> init() async => true;
diff --git a/lib/src/simulator/detector.dart b/lib/src/detector/sim_detector.dart
similarity index 88%
rename from lib/src/simulator/detector.dart
rename to lib/src/detector/sim_detector.dart
index 0b17967..dfac538 100644
--- a/lib/src/simulator/detector.dart
+++ b/lib/src/detector/sim_detector.dart
@@ -1,16 +1,15 @@
 import "package:autonomy/interfaces.dart";
-import "package:burt_network/generated.dart";
 
 class SimulatedObstacle {
   final GpsCoordinates coordinates;
   final int radius;
   SimulatedObstacle({required this.coordinates, required this.radius});
 
-  bool isNear(GpsCoordinates other) => 
+  bool isNear(GpsCoordinates other) =>
     coordinates.distanceTo(other) <= radius;
 }
 
-class DetectorSimulator extends DetectorInterface {  
+class DetectorSimulator extends DetectorInterface {
   static const arucoPosition = (10, 10);
   static const slopedLatitude = -5;
 
@@ -24,7 +23,7 @@ class DetectorSimulator extends DetectorInterface {
 
   @override
   Future<void> dispose() async => obstacles.clear();
-  
+
   @override
   bool findObstacles() {
     final coordinates = collection.gps.coordinates;
diff --git a/lib/src/drive/drive_commands.dart b/lib/src/drive/drive_commands.dart
new file mode 100644
index 0000000..94f8889
--- /dev/null
+++ b/lib/src/drive/drive_commands.dart
@@ -0,0 +1,39 @@
+import "package:autonomy/interfaces.dart";
+
+mixin RoverDriveCommands on DriveInterface {
+  /// Sets the max speed of the rover.
+  ///
+  /// [_setSpeeds] takes the speeds of each side of wheels. These numbers are percentages of the
+  /// max speed allowed by the rover, which we call the throttle. This function adjusts the
+  /// throttle, as a percentage of the rover's top speed.
+  void setThrottle(double throttle) {
+    collection.logger.trace("Setting throttle to $throttle");
+    sendCommand(DriveCommand(throttle: throttle, setThrottle: true));
+  }
+
+  /// Sets the speeds of the left and right wheels, using differential steering.
+  ///
+  /// These values are percentages of the max speed allowed by the rover. See [setThrottle].
+  void _setSpeeds({required double left, required double right}) {
+    right *= -1;
+    collection.logger.trace("Setting speeds to $left and $right");
+    sendCommand(DriveCommand(left: left, setLeft: true));
+    sendCommand(DriveCommand(right: right, setRight: true));
+  }
+
+  void stopMotors() {
+    setThrottle(0);
+    _setSpeeds(left: 0, right: 0);
+  }
+
+  void spinLeft() => _setSpeeds(left: -1, right: 1);
+  void spinRight() => _setSpeeds(left: 1, right: -1);
+  void moveForward() => _setSpeeds(left: 1, right: 1);
+
+  /// Sets the angle of the front camera.
+  void setCameraAngle({required double swivel, required double tilt}) {
+    collection.logger.trace("Setting camera angles to $swivel (swivel) and $tilt (tilt)");
+    final command = DriveCommand(frontSwivel: swivel, frontTilt: tilt);
+    sendCommand(command);
+  }
+}
diff --git a/lib/src/drive/drive_config.dart b/lib/src/drive/drive_config.dart
new file mode 100644
index 0000000..2977da6
--- /dev/null
+++ b/lib/src/drive/drive_config.dart
@@ -0,0 +1,40 @@
+import "dart:io";
+
+import "package:burt_network/burt_network.dart";
+
+class DriveConfig {
+  final double forwardThrottle;
+  final double turnThrottle;
+  final Duration turnDelay;
+  final Duration oneMeterDelay;
+  final String subsystemsAddress;
+
+  const DriveConfig({
+    required this.forwardThrottle,
+    required this.turnThrottle,
+    required this.turnDelay,
+    required this.oneMeterDelay,
+    required this.subsystemsAddress,
+  });
+
+  SocketInfo get subsystems => SocketInfo(
+    address: InternetAddress(subsystemsAddress),
+    port: 8001,
+  );
+}
+
+const roverConfig = DriveConfig(
+  forwardThrottle: 0.1,
+  turnThrottle: 0.1,
+  oneMeterDelay: Duration(milliseconds: 5500),
+  turnDelay: Duration(milliseconds: 4500),
+  subsystemsAddress: "192.168.1.20",
+);
+
+const tankConfig = DriveConfig(
+  forwardThrottle: 0.3,
+  turnThrottle: 0.35,
+  turnDelay: Duration(milliseconds: 1000),
+  oneMeterDelay: Duration(milliseconds: 2000),
+  subsystemsAddress: "localhost",
+);
diff --git a/lib/src/drive/drive_interface.dart b/lib/src/drive/drive_interface.dart
new file mode 100644
index 0000000..a1f3951
--- /dev/null
+++ b/lib/src/drive/drive_interface.dart
@@ -0,0 +1,55 @@
+import "package:autonomy/interfaces.dart";
+
+import "drive_config.dart";
+
+enum DriveDirection {
+  forward,
+  left,
+  right,
+  quarterLeft,
+  quarterRight,
+  stop;
+
+  bool get isTurn => this != forward && this != stop;
+}
+
+abstract class DriveInterface extends Service {
+  AutonomyInterface collection;
+  DriveInterface({required this.collection});
+
+  DriveConfig get config => roverConfig;
+
+  Future<void> stop();
+
+  Future<void> driveForward(GpsCoordinates position);
+
+  Future<void> faceDirection(CardinalDirection orientation);
+
+  void sendCommand(Message message) => collection.server
+    .sendMessage(message, destination: config.subsystems);
+
+  Future<void> resolveOrientation() => faceDirection(collection.imu.nearest);
+
+  /// Turns to face the state's [AutonomyAStarState.orientation].
+  ///
+  /// Exists so that the TimedDrive can implement this in terms of [AutonomyAStarState.instruction].
+  Future<void> turnState(AutonomyAStarState state) => faceDirection(state.orientation);
+
+  Future<void> driveState(AutonomyAStarState state) {
+    if (state.instruction == DriveDirection.stop) {
+      return stop();
+    } else if (state.instruction == DriveDirection.forward) {
+      return driveForward(state.position);
+    } else {
+      return turnState(state);
+    }
+  }
+
+  void setLedStrip(ProtoColor color, {bool blink = false}) {
+    final command = DriveCommand(color: color, blink: blink ? BoolState.YES : BoolState.NO);
+    sendCommand(command);
+  }
+
+  Future<bool> spinForAruco() async => false;
+  Future<void> approachAruco() async { }
+}
diff --git a/lib/src/drive/rover_drive.dart b/lib/src/drive/rover_drive.dart
new file mode 100644
index 0000000..434b9a5
--- /dev/null
+++ b/lib/src/drive/rover_drive.dart
@@ -0,0 +1,88 @@
+import "package:autonomy/interfaces.dart";
+import "package:autonomy/rover.dart";
+
+import "sensor_drive.dart";
+import "timed_drive.dart";
+import "sim_drive.dart";
+
+/// A helper class to send drive commands to the rover with a simpler API.
+class RoverDrive extends DriveInterface {
+  final bool useGps;
+  final bool useImu;
+
+  late final sensorDrive = SensorDrive(collection: collection);
+  late final timedDrive = TimedDrive(collection: collection);
+  late final simDrive = DriveSimulator(collection: collection);
+
+  RoverDrive({
+    required super.collection,
+    this.useGps = true,
+    this.useImu = true,
+  });
+
+  /// Initializes the rover's drive subsystems.
+  @override
+  Future<bool> init() async {
+    if (!useImu && collection.imu is RoverImu) {
+      collection.logger.critical(
+        "Cannot use Rover IMU with simulated turning",
+        body: "Set useImu to true, or use the simulated IMU",
+      );
+      return false;
+    }
+    if (!useGps && collection.imu is RoverGps) {
+      collection.logger.critical(
+        "Cannot use Rover GPS with simulated driving",
+        body: "Set useGps to true, or use the simulated GPS",
+      );
+      return false;
+    }
+
+    var result = true;
+    result &= await sensorDrive.init();
+    result &= await timedDrive.init();
+    result &= await simDrive.init();
+    return result;
+  }
+
+  /// Stops the rover from driving.
+  @override
+  Future<void> dispose() async {
+    await sensorDrive.dispose();
+    await timedDrive.dispose();
+    await simDrive.dispose();
+  }
+
+  @override
+  Future<void> stop() async {
+    await sensorDrive.stop();
+    await timedDrive.stop();
+    await simDrive.stop();
+  }
+
+  @override
+  Future<bool> spinForAruco() => sensorDrive.spinForAruco();
+
+  @override
+  Future<void> approachAruco() => sensorDrive.approachAruco();
+
+  @override
+  Future<void> faceDirection(CardinalDirection orientation) async {
+    if (useImu) {
+      await sensorDrive.faceDirection(orientation);
+    } else {
+      await timedDrive.faceDirection(orientation);
+      await simDrive.faceDirection(orientation);
+    }
+  }
+
+  @override
+  Future<void> driveForward(GpsCoordinates position) async {
+    if (useGps) {
+      await sensorDrive.driveForward(position);
+    } else {
+      await timedDrive.driveForward(position);
+      await simDrive.driveForward(position);
+    }
+  }
+}
diff --git a/lib/src/drive/sensor_drive.dart b/lib/src/drive/sensor_drive.dart
new file mode 100644
index 0000000..22c03d3
--- /dev/null
+++ b/lib/src/drive/sensor_drive.dart
@@ -0,0 +1,87 @@
+import "package:autonomy/autonomy.dart";
+import "package:autonomy/interfaces.dart";
+
+import "drive_commands.dart";
+
+class SensorDrive extends DriveInterface with RoverDriveCommands {
+  static const predicateDelay = Duration(milliseconds: 10);
+
+  SensorDrive({required super.collection});
+
+  @override
+  Future<void> stop() async => stopMotors();
+
+  Future<void> waitFor(bool Function() predicate) async {
+    while (!predicate()) {
+      await Future<void>.delayed(predicateDelay);
+    }
+  }
+
+  @override
+  Future<bool> init() async => true;
+
+  @override
+  Future<void> dispose() async { }
+
+  @override
+  Future<void> driveForward(GpsCoordinates position) async {
+    collection.logger.info("Driving forward one meter");
+    setThrottle(config.forwardThrottle);
+    moveForward();
+    await waitFor(() => collection.gps.isNear(position));
+    await stop();
+  }
+
+  @override
+  Future<void> faceDirection(CardinalDirection orientation) async {
+    collection.logger.info("Turning to face $orientation...");
+    setThrottle(config.turnThrottle);
+    await waitFor(() => _tryToFace(orientation));
+    await stop();
+  }
+
+  bool _tryToFace(CardinalDirection orientation) {
+    final current = collection.imu.heading;
+    final target = orientation.angle;
+    if ((current - target).abs() < 180) {
+      if (current < target) {
+        spinRight();
+      } else {
+        spinLeft();
+      }
+    } else {
+      if (current < target) {
+        spinLeft();
+      } else {
+        spinRight();
+      }
+    }
+    collection.logger.trace("Current heading: $current");
+    return collection.imu.isNear(orientation);
+  }
+
+  @override
+  Future<bool> spinForAruco() async {
+    setThrottle(config.turnThrottle);
+    spinLeft();
+    final result = await waitFor(() => collection.detector.canSeeAruco())
+      .then((_) => true)
+      .timeout(config.turnDelay * 4, onTimeout: () => false);
+    await stop();
+    return result;
+  }
+
+  @override
+  Future<void> approachAruco() async {
+    const sizeThreshold = 0.2;
+    const epsilon = 0.00001;
+    setThrottle(config.forwardThrottle);
+    moveForward();
+    await waitFor(() {
+      final size = collection.video.arucoSize;
+      collection.logger.trace("The Aruco tag is at $size percent");
+      return (size.abs() < epsilon && !collection.detector.canSeeAruco()) || size >= sizeThreshold;
+    }).timeout(config.oneMeterDelay * 5);
+    await stop();
+  }
+}
diff --git a/lib/src/drive/sim_drive.dart b/lib/src/drive/sim_drive.dart
new file mode 100644
index 0000000..35d6cb2
--- /dev/null
+++ b/lib/src/drive/sim_drive.dart
@@ -0,0 +1,29 @@
+import "package:autonomy/interfaces.dart";
+
+class DriveSimulator extends DriveInterface {
+  static const delay = Duration(milliseconds: 500);
+
+  final bool shouldDelay;
+  DriveSimulator({required super.collection, this.shouldDelay = false});
+
+  @override
+  Future<bool> init() async => true;
+
+  @override
+  Future<void> dispose() async { }
+
+  @override
+  Future<void> driveForward(GpsCoordinates position) async {
+    if (shouldDelay) await Future<void>.delayed(delay);
+    collection.gps.update(position);
+  }
+
+  @override
+  Future<void> faceDirection(CardinalDirection orientation) async {
+    if (shouldDelay) await Future<void>.delayed(const Duration(milliseconds: 500));
+    collection.imu.update(orientation.orientation);
+  }
+
+  @override
+  Future<void> stop() async => collection.logger.debug("Stopping");
+}
diff --git a/lib/src/drive/timed_drive.dart b/lib/src/drive/timed_drive.dart
new file mode 100644
index 0000000..1ab997a
--- /dev/null
+++ b/lib/src/drive/timed_drive.dart
@@ -0,0 +1,79 @@
+import "dart:math";
+
+import "package:autonomy/interfaces.dart";
+
+import "drive_commands.dart";
+
+class TimedDrive extends DriveInterface with RoverDriveCommands {
+  TimedDrive({required super.collection});
+
+  @override
+  Future<bool> init() async => true;
+
+  @override
+  Future<void> dispose() async { }
+
+  @override
+  Future<void> driveForward(GpsCoordinates position) async {
+    await goForward(collection.imu.nearest.isPerpendicular ? 1 : sqrt2);
+  }
+
+  @override
+  Future<void> turnState(AutonomyAStarState state) async {
+    switch (state.instruction) {
+      case DriveDirection.left:
+        await turnLeft();
+      case DriveDirection.right:
+        await turnRight();
+      case DriveDirection.quarterLeft:
+        await turnQuarterLeft();
+      case DriveDirection.quarterRight:
+        await turnQuarterRight();
+      case DriveDirection.forward || DriveDirection.stop:
+        break;
+    }
+  }
+
+  @override
+  Future<void> stop() async => stopMotors();
+
+  Future<void> goForward([double distance = 1]) async {
+    collection.logger.info("Driving forward $distance meters");
+    setThrottle(config.forwardThrottle);
+    moveForward();
+    await Future<void>.delayed(config.oneMeterDelay * distance);
+    await stop();
+  }
+
+  Future<void> turnLeft() async {
+    setThrottle(config.turnThrottle);
+    spinLeft();
+    await Future<void>.delayed(config.turnDelay);
+    await stop();
+  }
+
+  Future<void> turnRight() async {
+    setThrottle(config.turnThrottle);
+    spinRight();
+    await Future<void>.delayed(config.turnDelay);
+    await stop();
+  }
+
+  Future<void> turnQuarterLeft() async {
+    setThrottle(config.turnThrottle);
+    spinLeft();
+    await Future<void>.delayed(config.turnDelay * 0.5);
+    await stop();
+  }
+
+  Future<void> turnQuarterRight() async {
+    setThrottle(config.turnThrottle);
+    spinRight();
+    await Future<void>.delayed(config.turnDelay * 0.5);
+    await stop();
+  }
+
+  @override
+  Future<void> faceDirection(CardinalDirection orientation) =>
+    throw UnsupportedError("Cannot face any arbitrary direction using TimedDrive");
+}
diff --git a/lib/src/interfaces/gps.dart b/lib/src/gps/gps_interface.dart
similarity index 93%
rename from lib/src/interfaces/gps.dart
rename to lib/src/gps/gps_interface.dart
index 953b8b0..e5f8ae7 100644
--- a/lib/src/interfaces/gps.dart
+++ b/lib/src/gps/gps_interface.dart
@@ -1,4 +1,3 @@
-import "package:burt_network/generated.dart";
 import "package:autonomy/interfaces.dart";
 
 abstract class GpsInterface extends Service with Receiver {
diff --git a/lib/src/gps/rover_gps.dart b/lib/src/gps/rover_gps.dart
new file mode 100644
index 0000000..80a1cb0
--- /dev/null
+++ b/lib/src/gps/rover_gps.dart
@@ -0,0 +1,42 @@
+import "package:autonomy/interfaces.dart";
+
+class RoverGps extends GpsInterface {
+  final _latitudeCorrector = ErrorCorrector.disabled();
+  final _longitudeCorrector = ErrorCorrector.disabled();
+  RoverGps({required super.collection});
+
+  @override
+  Future<bool> init() async {
+    collection.server.messages.onMessage(
+      name: RoverPosition().messageName,
+      constructor: RoverPosition.fromBuffer,
+      callback: _internalUpdate,
+    );
+    return super.init();
+  }
+
+  @override
+  Future<void> dispose() async {
+    _latitudeCorrector.clear();
+    _longitudeCorrector.clear();
+  }
+
+  @override
+  void update(GpsCoordinates newValue) {
+    // Do nothing, since this should only be internally updated
+  }
+
+  void _internalUpdate(RoverPosition newValue) {
+    if (!newValue.hasGps()) return;
+    final position = newValue.gps;
+    _latitudeCorrector.addValue(position.latitude);
+    _longitudeCorrector.addValue(position.longitude);
+    hasValue = true;
+  }
+
+  @override
+  GpsCoordinates get coordinates => GpsCoordinates(
+    latitude: _latitudeCorrector.calibratedValue,
+    longitude: _longitudeCorrector.calibratedValue,
+  );
+}
diff --git a/lib/src/simulator/gps.dart b/lib/src/gps/sim_gps.dart
similarity index 93%
rename from lib/src/simulator/gps.dart
rename to lib/src/gps/sim_gps.dart
index 48d652f..d90a7f5 100644
--- a/lib/src/simulator/gps.dart
+++ b/lib/src/gps/sim_gps.dart
@@ -1,4 +1,3 @@
-import "package:burt_network/generated.dart";
 import "package:autonomy/interfaces.dart";
 
 class GpsSimulator extends GpsInterface with ValueReporter {
diff --git a/lib/src/imu/cardinal_direction.dart b/lib/src/imu/cardinal_direction.dart
new file mode 100644
index 0000000..40b2521
--- /dev/null
+++ b/lib/src/imu/cardinal_direction.dart
@@ -0,0 +1,79 @@
+
+import "package:autonomy/interfaces.dart";
+
+enum CardinalDirection {
+  north(0),
+  west(90),
+  south(180),
+  east(270),
+  northEast(000 + 45),
+  northWest(360 - 45),
+  southWest(180 + 45),
+  southEast(180 - 45);
+
+  final double angle;
+  const CardinalDirection(this.angle);
+
+  Orientation get orientation => Orientation(z: angle);
+
+  static CardinalDirection nearest(Orientation orientation) {
+    var smallestDiff = double.infinity;
+    var closestOrientation = CardinalDirection.north;
+
+    for (final value in values) {
+      final diff = (value.angle - orientation.z).clampAngle();
+      if (diff < smallestDiff) {
+        smallestDiff = diff;
+        closestOrientation = value;
+      }
+    }
+
+    return closestOrientation;
+  }
+
+  bool get isPerpendicular => angle.abs() % 90 == 0;
+
+  CardinalDirection turnLeft() => switch (this) {
+    north => west,
+    west => south,
+    south => east,
+    east => north,
+    northEast => northWest,
+    northWest => southWest,
+    southWest => southEast,
+    southEast => northEast,
+  };
+
+  CardinalDirection turnRight() => switch (this) {
+    north => east,
+    west => north,
+    south => west,
+    east => south,
+    northEast => southEast,
+    southEast => southWest,
+    southWest => northWest,
+    northWest => northEast,
+  };
+
+  CardinalDirection turnQuarterLeft() => switch (this) {
+    north => northWest,
+    northWest => west,
+    west => southWest,
+    southWest => south,
+    south => southEast,
+    southEast => east,
+    east => northEast,
+    northEast => north,
+  };
+
+  CardinalDirection turnQuarterRight() => switch (this) {
+    north => northEast,
+    northEast => east,
+    east => southEast,
+    southEast => south,
+    south => southWest,
+    southWest => west,
+    west => northWest,
+    northWest => north,
+  };
+}
diff --git a/lib/src/interfaces/imu.dart b/lib/src/imu/imu_interface.dart
similarity index 57%
rename from lib/src/interfaces/imu.dart
rename to lib/src/imu/imu_interface.dart
index 5bb6bba..7fbbf83 100644
--- a/lib/src/interfaces/imu.dart
+++ b/lib/src/imu/imu_interface.dart
@@ -1,5 +1,4 @@
 import "package:autonomy/interfaces.dart";
-import "package:burt_network/burt_network.dart";
 
 abstract class ImuInterface extends Service with Receiver {
   final AutonomyInterface collection;
@@ -7,12 +6,11 @@ abstract class ImuInterface extends Service with Receiver {
 
   double get heading => raw.z;
   Orientation get raw;
-  DriveOrientation? get orientation {
-	collection.logger.trace("Trying to find orientation at $heading");
-	return DriveOrientation.fromRaw(raw);
-  }
+
+  CardinalDirection get nearest => CardinalDirection.nearest(raw);
+
   void update(Orientation newValue);
-  bool isNear(double angle) => raw.isNear(angle);
+  bool isNear(CardinalDirection direction) => raw.isNear(direction.angle);
 
   @override
   Future<bool> init() async => true;
diff --git a/lib/src/imu/rover_imu.dart b/lib/src/imu/rover_imu.dart
new file mode 100644
index 0000000..2d9d63a
--- /dev/null
+++ b/lib/src/imu/rover_imu.dart
@@ -0,0 +1,43 @@
+import "package:autonomy/interfaces.dart";
+
+class RoverImu extends ImuInterface {
+  final _xCorrector = ErrorCorrector.disabled();
+  final _yCorrector = ErrorCorrector.disabled();
+  final _zCorrector = ErrorCorrector.disabled();
+  RoverImu({required super.collection});
+
+  @override
+  Future<bool> init() async {
+    collection.server.messages.onMessage(
+      name: RoverPosition().messageName,
+      constructor: RoverPosition.fromBuffer,
+      callback: _internalUpdate,
+    );
+    return super.init();
+  }
+
+  @override
+  Future<void> dispose() async {
+    _zCorrector.clear();
+  }
+
+  @override
+  void update(Orientation newValue) {
+    // Do nothing, since this should only be internally updated
+  }
+
+  void _internalUpdate(RoverPosition newValue) {
+    if (!newValue.hasOrientation()) return;
+    _xCorrector.addValue(newValue.orientation.x);
+    _yCorrector.addValue(newValue.orientation.y);
+    _zCorrector.addValue(newValue.orientation.z);
+    hasValue = true;
+  }
+
+  @override
+  Orientation get raw => Orientation(
+    x: _xCorrector.calibratedValue.clampAngle(),
+    y: _yCorrector.calibratedValue.clampAngle(),
+    z: _zCorrector.calibratedValue.clampAngle(),
+  );
+}
diff --git a/lib/src/simulator/imu.dart b/lib/src/imu/sim_imu.dart
similarity index 77%
rename from lib/src/simulator/imu.dart
rename to lib/src/imu/sim_imu.dart
index da7678a..8798446 100644
--- a/lib/src/simulator/imu.dart
+++ b/lib/src/imu/sim_imu.dart
@@ -1,11 +1,10 @@
-import "package:burt_network/generated.dart";
 import "package:autonomy/interfaces.dart";
 
-class ImuSimulator extends ImuInterface with ValueReporter {  
+class ImuSimulator extends ImuInterface with ValueReporter {
   final RandomError _error;
-  ImuSimulator({required super.collection, double maxError = 0}) : 
+  ImuSimulator({required super.collection, double maxError = 0}) :
     _error = RandomError(maxError);
-  
+
   @override
   RoverPosition getMessage() => RoverPosition(orientation: raw);
 
@@ -19,7 +18,7 @@ class ImuSimulator extends ImuInterface with ValueReporter {
   );
 
   @override
-  void update(Orientation newValue) => _orientation = newValue.clampHeading();
+  void update(Orientation newValue) => _orientation = newValue;
 
   @override
   Future<bool> init() async {
diff --git a/lib/src/interfaces/a_star.dart b/lib/src/interfaces/a_star.dart
deleted file mode 100644
index a0177d3..0000000
--- a/lib/src/interfaces/a_star.dart
+++ /dev/null
@@ -1,167 +0,0 @@
-import "dart:math";
-
-import "package:a_star/a_star.dart";
-
-import "package:burt_network/generated.dart";
-import "package:autonomy/interfaces.dart";
-
-class AutonomyAStarState extends AStarState<AutonomyAStarState> {
-  final DriveDirection direction;
-  final DriveOrientation orientation;
-  final GpsCoordinates position;
-  final GpsCoordinates goal;
-  final AutonomyInterface collection;
-
-  AutonomyAStarState({
-    required this.position, 
-    required this.goal, 
-    required this.collection,
-    required this.direction,
-    required this.orientation,
-    required super.depth,
-  });
-
-  factory AutonomyAStarState.start({
-    required AutonomyInterface collection,
-    required GpsCoordinates goal,
-  }) => AutonomyAStarState(
-    position: collection.gps.coordinates, 
-    goal: goal, 
-    collection: collection, 
-    direction: DriveDirection.stop, 
-    orientation: collection.imu.orientation ?? DriveOrientation.north, 
-    depth: 0,
-  );
-
-  @override
-  String toString() => switch(direction) {
-    DriveDirection.forward => "Go forward to ${position.prettyPrint()}",
-    DriveDirection.left => "Turn left to face $direction",
-    DriveDirection.right => "Turn right to face $direction",
-    DriveDirection.stop => "Start/Stop at ${position.prettyPrint()}",
-    DriveDirection.forwardLeft => "Turn 45 degrees left to face $direction",
-    DriveDirection.forwardRight => "Turn 45 degrees right to face $direction",
-  };
-
-  @override
-  double heuristic() => position.distanceTo(goal);
-
-  @override
-  String hash() => "${position.prettyPrint()} ($orientation)";
-
-  @override
-  bool isGoal() => position.isNear(goal, min(GpsUtils.moveLengthMeters, GpsUtils.maxErrorMeters));
-
-  AutonomyAStarState copyWith({required DriveDirection direction, required DriveOrientation orientation, required GpsCoordinates position}) => AutonomyAStarState(
-    collection: collection,
-    position: position,
-    orientation: orientation, 
-    direction: direction,
-    goal: goal, 
-    depth: (direction == DriveDirection.forward)
-        ? depth + 1
-        : (direction == DriveDirection.forwardLeft || direction == DriveDirection.forwardRight)
-            ? depth + sqrt2
-            : depth + 2 * sqrt2,
-  );
-
-  /// Returns whether or not the rover will drive between or right next to an obstacle diagonally<br/>
-  /// <br/>
-  /// Case 1:<br/>
-  /// 0 X<br/>
-  /// X R<br/>
-  /// Assuming the rover is facing 0 and trying to drive forward, will return false<br/>
-  /// <br/>
-  /// Case 2:<br/>
-  /// 0 X<br/>
-  /// X R<br/>
-  /// Assuming the rover is facing north and trying to turn 45 degrees left, will return false<br/>
-  /// <br/>
-  /// Case 3:<br/>
-  /// 0 X<br/>
-  /// 0 R<br/>
-  /// If the rover is facing left but trying to turn 45 degrees right, will return false<br/>
-  /// <br/>
-  /// Case 4:<br/>
-  /// 0 X 0<br/>
-  /// 0 R 0<br/>
-  /// If the rover is facing northeast to 0 and trying to turn left, will return false
-  bool drivingThroughObstacle(AutonomyAStarState state) {
-    final isTurn = state.direction != DriveDirection.forward;
-    final isQuarterTurn = state.direction == DriveDirection.forwardLeft || state.direction == DriveDirection.forwardRight;
-
-    // Forward drive across the perpendicular axis
-    if (!isTurn && state.orientation.isPerpendicular) {
-      return false;
-    }
-
-    // Not encountering any sort of diagonal angle
-    if (isTurn && isQuarterTurn && state.orientation.isPerpendicular) {
-      return false;
-    }
-
-    // No diagonal movement, won't drive between obstacles
-    if (!isQuarterTurn && orientation.isPerpendicular) {
-      return false;
-    }
-
-    DriveOrientation orientation1;
-    DriveOrientation orientation2;
-
-    // Case 1, trying to drive while facing a 45 degree angle
-    if (!isTurn) {
-      orientation1 = state.orientation.turnQuarterLeft();
-      orientation2 = state.orientation.turnQuarterRight();
-    } else if (isQuarterTurn) { // Case 2 and Case 3
-      orientation1 = orientation;
-      orientation2 = (state.direction == DriveDirection.forwardLeft)
-          ? orientation1.turnLeft()
-          : orientation1.turnRight();
-    } else { // Case 4
-      orientation1 = (state.direction == DriveDirection.left)
-          ? orientation.turnQuarterLeft()
-          : orientation.turnQuarterRight();
-      orientation2 = (state.direction == DriveDirection.left)
-          ? state.orientation.turnQuarterLeft()
-          : state.orientation.turnQuarterRight();
-    }
-
-    // Since the state being passed has a position of moving after the
-    // turn, we have to check the position of where it started
-    return collection.pathfinder.isObstacle(
-          position.goForward(orientation1),
-        ) ||
-        collection.pathfinder.isObstacle(
-          position.goForward(orientation2),
-        );
-  }
-
-  @override
-  Iterable<AutonomyAStarState> expand() => [
-        copyWith(
-          direction: DriveDirection.forward,
-          orientation: orientation,
-          position: position.goForward(orientation),
-        ),
-        copyWith(
-          direction: DriveDirection.left,
-          orientation: orientation.turnLeft(),
-          position: position,
-        ),
-        copyWith(
-          direction: DriveDirection.right,
-          orientation: orientation.turnRight(),
-          position: position,
-        ),
-        copyWith(
-          direction: DriveDirection.forwardLeft,
-          orientation: orientation.turnQuarterLeft(),
-          position: position,
-        ),
-        copyWith(
-          direction: DriveDirection.forwardRight,
-          orientation: orientation.turnQuarterRight(),
-          position: position,
-        ),
-      ].where((state) => !collection.pathfinder.isObstacle(state.position) && !drivingThroughObstacle(state));
-}
diff --git a/lib/src/interfaces/drive.dart b/lib/src/interfaces/drive.dart
deleted file mode 100644
index f9b6c96..0000000
--- a/lib/src/interfaces/drive.dart
+++ /dev/null
@@ -1,125 +0,0 @@
-import "package:autonomy/interfaces.dart";
-import "package:burt_network/generated.dart";
-
-const bool isRover = false;
-
-enum DriveDirection {
-  forward, 
-  left,
-  right,
-  forwardLeft,
-  forwardRight,
-  stop;
-
-  bool get isTurn => this != forward && this != stop;
-}
-
-enum DriveOrientation {
-  north(0),
-  west(90),
-  south(180),
-  east(270),
-  northEast(360 - 45),
-  northWest(45),
-  southEast(180 + 45),
-  southWest(180 - 45);
-
-  final int angle;
-  const DriveOrientation(this.angle);
-
-  static DriveOrientation? fromRaw(Orientation orientation) {
-    // TODO: Make this more precise.
-    for (final value in values) {
-      if (orientation.isNear(value.angle.toDouble(), OrientationUtils.orientationEpsilon)) return value;
-    }
-    return null;
-  }
-
-  bool get isPerpendicular => angle.abs() % 90 == 0;
-
-  DriveOrientation turnLeft() => switch (this) {
-    north => west,
-    west => south,
-    south => east,
-    east => north,
-    northEast => northWest,
-    northWest => southWest,
-    southWest => southEast,
-    southEast => northEast,
-  };
-
-  DriveOrientation turnRight() => switch (this) {
-    north => east,
-    west => north,
-    south => west,
-    east => south,
-    northEast => southEast,
-    southEast => southWest,
-    southWest => northWest,
-    northWest => northEast,
-  };
-
-  DriveOrientation turnQuarterLeft() => switch (this) {
-    north => northWest,
-    northWest => west,
-    west => southWest,
-    southWest => south,
-    south => southEast,
-    southEast => east,
-    east => northEast,
-    northEast => north,
-  };
-
-  DriveOrientation turnQuarterRight() => switch (this) {
-    north => northEast,
-    northEast => east,
-    east => southEast,
-    southEast => south,
-    south => southWest,
-    southWest => west,
-    west => northWest,
-    northWest => north,
-  };
-}
-
-abstract class DriveInterface extends Service {
-  AutonomyInterface collection;
-  DriveOrientation orientation = DriveOrientation.north;
-  DriveInterface({required this.collection});
-
-  Future<void> goDirection(DriveDirection direction) async => switch (direction) {
-    DriveDirection.forward => await goForward(),
-    DriveDirection.left => await turnLeft(),
-    DriveDirection.right => await turnRight(),
-    DriveDirection.forwardLeft => await turnQuarterLeft(),
-    DriveDirection.forwardRight => await turnQuarterRight(),
-    DriveDirection.stop => await stop(),
-  };
-
-  Future<void> faceNorth();
-  
-  Future<void> goForward();
-  Future<void> turnLeft();
-  Future<void> turnRight();
-  Future<void> turnQuarterLeft();
-  Future<void> turnQuarterRight();
-  Future<void> stop();
-
-  Future<void> faceDirection(DriveOrientation orientation) async {
-    this.orientation = orientation;
-  }
-
-  Future<void> followPath(List<AutonomyAStarState> path) async {
-    for (final state in path) {
-      await goDirection(state.direction);
-    }
-  }
-
-  void setLedStrip(ProtoColor color, {bool blink = false}) {
-    final command = DriveCommand(color: color, blink: blink ? BoolState.YES : BoolState.NO);
-    collection.server.sendCommand(command);
-  }
-
-  Future<bool> spinForAruco() async => false;
-  Future<void> approachAruco() async { }
-}
diff --git a/lib/src/interfaces/imu_utils.dart b/lib/src/interfaces/imu_utils.dart
deleted file mode 100644
index c0f8a37..0000000
--- a/lib/src/interfaces/imu_utils.dart
+++ /dev/null
@@ -1,28 +0,0 @@
-import "package:burt_network/generated.dart";
-
-extension OrientationUtils on Orientation {
-  static const double epsilon = 3.5;
-  static const double orientationEpsilon = 10;
-  
-  static final north = Orientation(z: 0);
-  static final west = Orientation(z: 90);
-  static final south = Orientation(z: 180);
-  static final east = Orientation(z: 270);
-  
-  double get heading => z;
-  
-  bool get isEmpty => x == 0 && y == 0 && z == 0;
-  
-  Orientation clampHeading() {
-    var adjustedHeading = heading;
-    if (heading >= 360) adjustedHeading -= 360;
-    if (heading < 0) adjustedHeading = 360 + heading;
-    return Orientation(x: x, y: y, z: adjustedHeading);
-  }
-
-  bool isNear(double value, [double tolerance = epsilon]) => value > 270 && z < 90
-    ? (z + 360 - value).abs() < tolerance
-    : value < 90 && z > 270 
-      ? (z - value - 360).abs() < tolerance
-      : (z - value).abs() < tolerance;
-}
diff --git a/lib/src/interfaces/receiver.dart b/lib/src/interfaces/receiver.dart
deleted file mode 100644
index 59ec05b..0000000
--- a/lib/src/interfaces/receiver.dart
+++ /dev/null
@@ -1,9 +0,0 @@
-mixin Receiver {
-  bool hasValue = false;
-  
-  Future<void> waitForValue() async {
-    while (!hasValue) {
-      await Future<void>.delayed(const Duration(seconds: 1));
-    }
-  }
-}
diff --git a/lib/src/interfaces/server.dart b/lib/src/interfaces/server.dart
deleted file mode 100644
index cec6850..0000000
--- a/lib/src/interfaces/server.dart
+++ /dev/null
@@ -1,25 +0,0 @@
-import "dart:io";
-
-import "package:burt_network/burt_network.dart";
-
-extension ServerUtils on RoverSocket {
-  static SocketInfo subsystemsDestination = SocketInfo(
-    address: InternetAddress("192.168.1.20"),
-    port: 8001,
-  );
-
-  void sendCommand(Message message) => sendMessage(message, destination: subsystemsDestination);
-
-  Future<void> waitForConnection() async {
-    logger.info("Waiting for connection...");
-    while (!isConnected) {
-      await Future<void>.delayed(const Duration(milliseconds: 100));
-    }
-    return;
-  }
-
-  void sendDone() {
-    final message = AutonomyData(state: AutonomyState.AT_DESTINATION, task: AutonomyTask.AUTONOMY_TASK_UNDEFINED);
-    sendMessage(message);
-  }
-}
diff --git a/lib/src/interfaces/service.dart b/lib/src/interfaces/service.dart
deleted file mode 100644
index 19cbaca..0000000
--- a/lib/src/interfaces/service.dart
+++ /dev/null
@@ -1 +0,0 @@
-export "package:burt_network/burt_network.dart" show Service;
diff --git a/lib/src/interfaces/orchestrator.dart b/lib/src/orchestrator/orchestrator_interface.dart
similarity index 83%
rename from lib/src/interfaces/orchestrator.dart
rename to lib/src/orchestrator/orchestrator_interface.dart
index a1dc437..ce8fcfe 100644
--- a/lib/src/interfaces/orchestrator.dart
+++ b/lib/src/orchestrator/orchestrator_interface.dart
@@ -1,7 +1,6 @@
 import "dart:io";
 
 import "package:autonomy/interfaces.dart";
-import "package:burt_network/burt_network.dart";
 import "package:meta/meta.dart";
 
 abstract class OrchestratorInterface extends Service {
@@ -18,24 +17,25 @@ abstract class OrchestratorInterface extends Service {
       return;
     }
 
-    if (!collection.hasValue && false) {
+    if (!collection.hasValue) {
+      // We don't wait here because this was explicitly requested by the operator.
+      // Users expect immediate feedback, so we give an error instead of freezing.
       collection.logger.error("Sensors haven't gotten any readings yet!");
       currentState = AutonomyState.NO_SOLUTION;
       return;
     }
-    await collection.drive.faceNorth();
+    await collection.drive.resolveOrientation();
     currentCommand = command;
     switch (command.task) {
+      case AutonomyTask.BETWEEN_GATES: break;  // TODO
+      case AutonomyTask.AUTONOMY_TASK_UNDEFINED: break;
       case AutonomyTask.GPS_ONLY: await handleGpsTask(command);
       case AutonomyTask.VISUAL_MARKER: await handleArucoTask(command);
-      // TODO: Add more tasks
-      default: collection.logger.error("Unrecognized task: ${command.task}");  // ignore: no_default_cases
     }
   }
 
   @override
   Future<bool> init() async {
-    print("Orchestrator init 2");
     collection.server.messages.onMessage(
       name: AutonomyCommand().messageName,
       constructor: AutonomyCommand.fromBuffer,
diff --git a/lib/src/rover/orchestrator.dart b/lib/src/orchestrator/rover_orchestrator.dart
similarity index 93%
rename from lib/src/rover/orchestrator.dart
rename to lib/src/orchestrator/rover_orchestrator.dart
index c33e1e9..7d536c8 100644
--- a/lib/src/rover/orchestrator.dart
+++ b/lib/src/orchestrator/rover_orchestrator.dart
@@ -1,5 +1,4 @@
 import "package:autonomy/interfaces.dart";
-import "package:burt_network/generated.dart";
 import "dart:async";
 
 class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
@@ -36,17 +35,16 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
   @override
   Future<void> handleGpsTask(AutonomyCommand command) async {
     final destination = command.destination;
-    collection.logger.info("Got GPS Task: Go to ${destination.prettyPrint()}");
+    collection.logger.info("Got GPS Task", body: "Go to ${destination.prettyPrint()}");
     collection.logger.debug("Currently at ${collection.gps.coordinates.prettyPrint()}");
     traversed.clear();
     collection.drive.setLedStrip(ProtoColor.RED);
     collection.detector.findObstacles();
-    // await collection.drive.faceNorth();
+    await collection.drive.resolveOrientation();
     while (!collection.gps.coordinates.isNear(destination)) {
       // Calculate a path
       collection.logger.debug("Finding a path");
       currentState = AutonomyState.PATHING;
-      // await collection.drive.faceNorth();
       final path = collection.pathfinder.getPath(destination);
       currentPath = path;  // also use local variable path for promotion
       if (path == null) {
@@ -67,7 +65,7 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
       var count = 0;
       for (final state in path) {
         collection.logger.debug(state.toString());
-        await collection.drive.goDirection(state.direction);
+        await collection.drive.driveState(state);
         traversed.add(state.position);
         // if (state.direction != DriveDirection.forward) continue;
         if (count++ == 5) break;
diff --git a/lib/src/simulator/orchestrator.dart b/lib/src/orchestrator/sim_orchestrator.dart
similarity index 92%
rename from lib/src/simulator/orchestrator.dart
rename to lib/src/orchestrator/sim_orchestrator.dart
index 6df6e49..ae2dd75 100644
--- a/lib/src/simulator/orchestrator.dart
+++ b/lib/src/orchestrator/sim_orchestrator.dart
@@ -1,5 +1,4 @@
 import "package:autonomy/interfaces.dart";
-import "package:burt_network/generated.dart";
 
 class OrchestratorSimulator extends OrchestratorInterface {
   OrchestratorSimulator({required super.collection});
diff --git a/lib/src/interfaces/pathfinding.dart b/lib/src/pathfinding/pathfinding_interface.dart
similarity index 92%
rename from lib/src/interfaces/pathfinding.dart
rename to lib/src/pathfinding/pathfinding_interface.dart
index cc780c8..25bce8e 100644
--- a/lib/src/interfaces/pathfinding.dart
+++ b/lib/src/pathfinding/pathfinding_interface.dart
@@ -1,5 +1,4 @@
 import "package:autonomy/interfaces.dart";
-import "package:burt_network/generated.dart";
 
 abstract class PathfindingInterface extends Service {
   final AutonomyInterface collection;
diff --git a/lib/src/rover/pathfinding.dart b/lib/src/pathfinding/rover_pathfinding.dart
similarity index 73%
rename from lib/src/rover/pathfinding.dart
rename to lib/src/pathfinding/rover_pathfinding.dart
index a7235d8..a72d279 100644
--- a/lib/src/rover/pathfinding.dart
+++ b/lib/src/pathfinding/rover_pathfinding.dart
@@ -1,10 +1,8 @@
 import "package:a_star/a_star.dart";
 
 import "package:autonomy/interfaces.dart";
-import "package:burt_network/burt_network.dart";
-import "package:burt_network/generated.dart";
 
-class RoverPathfinder extends PathfindingInterface {  
+class RoverPathfinder extends PathfindingInterface {
   RoverPathfinder({required super.collection});
 
   @override
@@ -12,7 +10,7 @@ class RoverPathfinder extends PathfindingInterface {
 
   @override
   List<AutonomyAStarState>? getPath(GpsCoordinates destination, {bool verbose = false}) {
-    if (isObstacle(destination)) return null;    
+    if (isObstacle(destination)) return null;
     final state = AutonomyAStarState.start(collection: collection, goal: destination);
     final result = aStar(state, verbose: verbose, limit: 50000);
     if (result == null) return null;
diff --git a/lib/src/pathfinding/sim_pathfinding.dart b/lib/src/pathfinding/sim_pathfinding.dart
new file mode 100644
index 0000000..3df3970
--- /dev/null
+++ b/lib/src/pathfinding/sim_pathfinding.dart
@@ -0,0 +1,24 @@
+import "package:autonomy/interfaces.dart";
+
+class PathfindingSimulator extends PathfindingInterface {
+  static int i = 0;
+
+  PathfindingSimulator({required super.collection});
+
+  @override
+  Future<bool> init() async => true;
+
+  @override
+  List<AutonomyAStarState> getPath(GpsCoordinates destination, {bool verbose = false}) => [
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 0).toGps(), orientation: CardinalDirection.north, instruction: DriveDirection.right, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 0).toGps(), orientation: CardinalDirection.east, instruction: DriveDirection.forward, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 1).toGps(), orientation: CardinalDirection.east, instruction: DriveDirection.forward, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 2).toGps(), orientation: CardinalDirection.east, instruction: DriveDirection.left, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 2).toGps(), orientation: CardinalDirection.north, instruction: DriveDirection.forward, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 1, long: 2).toGps(), orientation: CardinalDirection.north, instruction: DriveDirection.forward, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 2).toGps(), orientation: CardinalDirection.north, instruction: DriveDirection.left, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 2).toGps(), orientation: CardinalDirection.west, instruction: DriveDirection.forward, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 1).toGps(), orientation: CardinalDirection.west, instruction: DriveDirection.right, collection: collection),
+    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 1).toGps(), orientation: CardinalDirection.north, instruction: DriveDirection.forward, collection: collection),
+  ];
+}
diff --git a/lib/src/rover/drive.dart b/lib/src/rover/drive.dart
deleted file mode 100644
index 7235668..0000000
--- a/lib/src/rover/drive.dart
+++ /dev/null
@@ -1,73 +0,0 @@
-import "package:burt_network/burt_network.dart";
-import "package:autonomy/interfaces.dart";
-
-import "drive/timed.dart";
-import "drive/sensor.dart";
-
-/// A helper class to send drive commands to the rover with a simpler API.
-class RoverDrive extends DriveInterface {
-  final bool useGps;
-  final bool useImu;
-
-  final SensorDrive sensorDrive;
-  final TimedDrive timedDrive;
-
-  // TODO: Calibrate these
-  RoverDrive({required super.collection, this.useGps = true, this.useImu = true}) :
-    sensorDrive = SensorDrive(collection: collection),
-    timedDrive = TimedDrive(collection: collection);
-
-	/// Initializes the rover's drive subsystems.
-	@override
-  Future<bool> init() async => true;
-
-	/// Stops the rover from driving.
-	@override
-  Future<void> dispose() => stop();
-
-	/// Sets the angle of the front camera.
-	void setCameraAngle({required double swivel, required double tilt}) {
-		collection.logger.trace("Setting camera angles to $swivel (swivel) and $tilt (tilt)");
-		final command = DriveCommand(frontSwivel: swivel, frontTilt: tilt);
-		collection.server.sendCommand(command);
-	}
-
-  @override
-  Future<void> stop() async {
-    await timedDrive.stop();
-  }
-
-  @override
-  Future<void> faceNorth() => useImu ? sensorDrive.faceNorth() : timedDrive.faceNorth();
-
-  @override
-  Future<bool> spinForAruco() => sensorDrive.spinForAruco();
-  @override
-  Future<void> approachAruco() => sensorDrive.approachAruco();
-
-
-  @override
-  Future<void> faceDirection(DriveOrientation orientation) async {
-    if (useImu) {
-      await sensorDrive.faceDirection(orientation);
-    } else {
-      await timedDrive.faceDirection(orientation);
-    }
-    await super.faceDirection(orientation);
-  }
-
-  @override
-  Future<void> goForward() => useGps ? sensorDrive.goForward() : timedDrive.goForward();
-
-  @override
-  Future<void> turnLeft() => useImu ? sensorDrive.turnLeft() : timedDrive.turnLeft();
-
-  @override
-  Future<void> turnRight() => useImu ? sensorDrive.turnRight() : timedDrive.turnRight();
-
-  @override
-  Future<void> turnQuarterLeft() => useImu ? sensorDrive.turnQuarterLeft() : timedDrive.turnQuarterLeft();
-
-  @override
-  Future<void> turnQuarterRight() => useImu ? sensorDrive.turnQuarterRight() : timedDrive.turnQuarterRight();
-}
diff --git a/lib/src/rover/drive/motors.dart b/lib/src/rover/drive/motors.dart
deleted file mode 100644
index 98710c2..0000000
--- a/lib/src/rover/drive/motors.dart
+++ /dev/null
@@ -1,26 +0,0 @@
-import "package:autonomy/interfaces.dart";
-import "package:burt_network/generated.dart";
-
-mixin RoverMotors {
-  AutonomyInterface get collection;
-  
-  /// Sets the max speed of the rover. 
-	/// 
-	/// [setSpeeds] takes the speeds of each side of wheels. These numbers are percentages of the
-	/// max speed allowed by the rover, which we call the throttle. This function adjusts the 
-	/// throttle, as a percentage of the rover's top speed. 
-	void setThrottle(double throttle) {
-		collection.logger.trace("Setting throttle to $throttle");
-		collection.server.sendCommand(DriveCommand(throttle: throttle, setThrottle: true));
-	}
-
-	/// Sets the speeds of the left and right wheels, using differential steering. 
-	/// 
-	/// These values are percentages of the max speed allowed by the rover. See [setThrottle].
-	void setSpeeds({required double left, required double right}) {
-		right *= -1;
-		collection.logger.trace("Setting speeds to $left and $right");
-		collection.server.sendCommand(DriveCommand(left: left, setLeft: true));
-		collection.server.sendCommand(DriveCommand(right: right, setRight: true));
-	}
-}
diff --git a/lib/src/rover/drive/sensor.dart b/lib/src/rover/drive/sensor.dart
deleted file mode 100644
index adaab3f..0000000
--- a/lib/src/rover/drive/sensor.dart
+++ /dev/null
@@ -1,219 +0,0 @@
-import "package:autonomy/autonomy.dart";
-import "package:autonomy/interfaces.dart";
-
-import "motors.dart";
-
-class SensorDrive extends DriveInterface with RoverMotors {
-  static const double maxThrottle = 0.1;
-  static const double turnThrottleRover = 0.1;
-  static const double turnThrottleTank = 0.35;
-
-  static double get turnThrottle => isRover ? turnThrottleRover : turnThrottleTank;
-
-  static const predicateDelay = Duration(milliseconds: 100);
-  static const turnDelay = Duration(milliseconds: 1500);
-
-  SensorDrive({required super.collection});
-
-  @override
-  Future<void> stop() async {
-    setThrottle(0);
-    setSpeeds(left: 0, right: 0);
-  }
-
-  Future<void> waitFor(bool Function() predicate) async {
-    while (!predicate()) {
-      await Future<void>.delayed(predicateDelay);
-      await collection.imu.waitForValue();
-    }
-  }
-
-  Future<void> runFeedback(bool Function() completed, [Duration period = predicateDelay]) async {
-    while (!completed()) {
-      await Future<void>.delayed(period);
-    }
-  }
-
-  @override
-  Future<bool> init() async => true;
-
-  @override
-  Future<void> dispose() async { }
-
-  @override
-  Future<void> goForward() async {
-   final orientation = collection.imu.orientation;
-    final currentCoordinates = collection.gps.coordinates;
-    final destination = currentCoordinates.goForward(orientation!);
-
-    setThrottle(maxThrottle);
-    setSpeeds(left: 1, right: 1);
-    await waitFor(() => collection.gps.coordinates.isNear(destination));
-    await stop();
-  }
-
-  @override
-  Future<void> faceNorth() async {
-    collection.logger.info("Turning to face north...");
-    setThrottle(turnThrottle);
-    // setSpeeds(left: -1, right: 1);
-    // await waitFor(() {
-    //   collection.logger.trace("Current heading: ${collection.imu.heading}");
-    //   return collection.imu.raw.isNear(0, OrientationUtils.orientationEpsilon);
-    // });
-    await faceDirection(DriveOrientation.north);
-    await stop();
-  }
-
-  @override
-  Future<void> faceDirection(DriveOrientation orientation) async {
-    collection.logger.info("Turning to face $orientation...");
-    setThrottle(turnThrottle);
-    setSpeeds(left: -1, right: 1);
-    await runFeedback(
-      () {
-        var delta = orientation.angle.toDouble() - collection.imu.raw.z;
-        if (delta < -180) {
-          delta += 360;
-        } else if (delta > 180) {
-          delta -= 360;
-        }
-
-        if (delta < 0) {
-          setSpeeds(left: 1, right: -1);
-        } else {
-          setSpeeds(left: -1, right: 1);
-        }
-        collection.logger.trace("Current heading: ${collection.imu.heading}");
-        return collection.imu.raw.isNear(orientation.angle.toDouble());
-      },
-      const Duration(milliseconds: 10),
-    );
-    await stop();
-    await super.faceDirection(orientation);
-  }
-
-  @override
-  Future<void> turnLeft() async {
-    await collection.imu.waitForValue();
-
-    if (collection.imu.orientation == null) {
-      await faceNorth();
-      await faceDirection(this.orientation);
-    }
-    await collection.imu.waitForValue();
-
-    final orientation = collection.imu.orientation;
-    final destination = orientation!.turnLeft(); // do NOT clamp!
-    print("Going from ${orientation} to ${destination}");
-    setThrottle(turnThrottle);
-    // setSpeeds(left: -1, right: 1);
-    // await waitFor(() => collection.imu.orientation == destination);
-    await faceDirection(destination);
-    await stop();
-    this.orientation = this.orientation.turnLeft();
-  }
-
-  @override
-  Future<void> turnRight() async {
-    await collection.imu.waitForValue();
-    if (collection.imu.orientation == null) {
-      await faceNorth();
-      await faceDirection(this.orientation);
-    }
-    await collection.imu.waitForValue();
-    final orientation = collection.imu.orientation;
-    final destination = orientation!.turnRight(); // do NOT clamp!
-    setThrottle(turnThrottle);
-    // setSpeeds(left: 1, right: -1);
-    // await waitFor(() => collection.imu.orientation == destination);
-    await faceDirection(destination);
-    await stop();
-    this.orientation = this.orientation.turnRight();
-  }
-
-  @override
-  Future<void> turnQuarterLeft() async {
-    await collection.imu.waitForValue();
-
-    if (collection.imu.orientation == null) {
-      await faceNorth();
-      await faceDirection(this.orientation);
-    }
-    await collection.imu.waitForValue();
-
-    final orientation = collection.imu.orientation;
-    final destination = orientation!.turnQuarterLeft(); // do NOT clamp!
-    print("Going from ${orientation} to ${destination}");
-    setThrottle(turnThrottle);
-    // setSpeeds(left: -1, right: 1);
-    // await waitFor(() => collection.imu.orientation == destination);
-    await faceDirection(destination);
-    await stop();
-    this.orientation = this.orientation.turnQuarterLeft();
-  }
-
-  @override
-  Future<void> turnQuarterRight() async {
-    await collection.imu.waitForValue();
-
-    if (collection.imu.orientation == null) {
-      await faceNorth();
-      await faceDirection(this.orientation);
-    }
-    await collection.imu.waitForValue();
-
-    final orientation = collection.imu.orientation;
-    final destination = orientation!.turnQuarterRight(); // do NOT clamp!
-    print("Going from ${orientation} to ${destination}");
-    setThrottle(turnThrottle);
-    // setSpeeds(left: 1, right: -1);
-    // await waitFor(() => collection.imu.orientation == destination);
-    await faceDirection(destination);
-    await stop();
-    this.orientation = this.orientation.turnQuarterRight();
-  }
-
-  @override
-  Future<bool> spinForAruco() async {
-    for (var i = 0; i < 16; i++) {
-      setThrottle(turnThrottle);
-      setSpeeds(left: -1, right: 1);
-      await Future<void>.delayed(turnDelay);
-      await stop();
-
-      for (var j = 0; j < 300; j++) {
-        await Future<void>.delayed(const Duration(milliseconds: 10));
-        collection.logger.trace("Can see aruco? ${collection.detector.canSeeAruco()}");
-
-        if (collection.detector.canSeeAruco()) {
-          // Spin a bit more to center it
-          // print("We found it!");
-          // setThrottle(0.1);
-          // setSpeeds(left: -1, right: 1);
-          // await waitFor(() {
-            // final pos = collection.video.arucoPosition;
-            // collection.logger.debug("aruco is at $pos");
-            // return pos > 0.2;
-          //	});
-          // await stop();
-        return true;
-      }}
-    }
-    return false;
-  }
-
-  @override
-  Future<void> approachAruco() async {
-    setThrottle(maxThrottle);
-    setSpeeds(left: 1, right: 1);
-    // const threshold = 0.2;
-    //  await waitFor(() {
-      //	final pos = collection.video.arucoSize;
-      //	collection.logger.debug("It is at $pos percent");
-      //	return (pos.abs() < 0.00001 && !collection.detector.canSeeAruco()) || pos >= threshold;
-    //	});
-    await Future<void>.delayed(const Duration(seconds: 10));
-    await stop();
-  }
-}
diff --git a/lib/src/rover/drive/timed.dart b/lib/src/rover/drive/timed.dart
deleted file mode 100644
index bba9d6e..0000000
--- a/lib/src/rover/drive/timed.dart
+++ /dev/null
@@ -1,86 +0,0 @@
-import "package:autonomy/interfaces.dart";
-
-import "motors.dart";
-
-class TimedDrive extends DriveInterface with RoverMotors {
-  static const maxThrottleTank = 0.3;
-  static const turnThrottleTank = 0.35;
-
-  static const maxThrottleRover = 0.1;
-  static const turnThrottleRover = 0.1;
-
-  static const oneMeterDelayRover = Duration(milliseconds: 5500);
-  static const turnDelayRover = Duration(milliseconds: 4500);
-
-  static const oneMeterDelayTank = Duration(milliseconds: 2000);
-  static const turnDelayTank = Duration(milliseconds: 1000);
-
-  static double get maxThrottle => isRover ? maxThrottleRover : maxThrottleTank;
-  static double get turnThrottle => isRover ? turnThrottleRover : turnThrottleTank;
-
-  static Duration get oneMeterDelay => isRover ? oneMeterDelayRover : oneMeterDelayTank;
-  static Duration get turnDelay => isRover ? turnDelayRover : turnDelayTank;
-
-  TimedDrive({required super.collection});
-
-  @override
-  Future<bool> init() async => true;
-
-  @override
-  Future<void> dispose() async { }
-
-  @override
-  Future<void> stop() async {
-    setThrottle(0);
-    setSpeeds(left: 0, right: 0);
-  }
-
-  @override
-  Future<void> faceNorth() async { /* Assume already facing north */ }
-
-  @override
-  Future<void> goForward() async {
-    setThrottle(maxThrottle);
-    setSpeeds(left: 1 * 0.9, right: 1);
-    final position = collection.gps.coordinates;
-    final orientation = collection.imu.orientation;
-    if (orientation != null) {
-      final newPosition = position.goForward(orientation);
-      collection.gps.update(newPosition);
-    }
-    await Future<void>.delayed(oneMeterDelay);
-    await stop();
-  }
-
-  @override
-  Future<void> turnLeft() async {
-    setThrottle(turnThrottle);
-    setSpeeds(left: -1 * 0.9, right: 1);
-    await Future<void>.delayed(turnDelay);
-    await stop();
-  }
-
-  @override
-  Future<void> turnRight() async {
-    setThrottle(turnThrottle);
-    setSpeeds(left: 1, right: -1);
-    await Future<void>.delayed(turnDelay);
-    await stop();
-  }
-
-  @override
-  Future<void> turnQuarterLeft() async {
-    setThrottle(turnThrottle);
-    setSpeeds(left: -1, right: 1);
-    await Future<void>.delayed(turnDelay * 0.5);
-    await stop();
-  }
-
-  @override
-  Future<void> turnQuarterRight() async {
-    setThrottle(turnThrottle);
-    setSpeeds(left: 1, right: -1);
-    await Future<void>.delayed(turnDelay * 0.5);
-    await stop();
-  }
-}
diff --git a/lib/src/rover/gps.dart b/lib/src/rover/gps.dart
deleted file mode 100644
index 2704b85..0000000
--- a/lib/src/rover/gps.dart
+++ /dev/null
@@ -1,41 +0,0 @@
-import "package:burt_network/burt_network.dart";
-import "package:autonomy/interfaces.dart";
-
-import "corrector.dart";
-
-class RoverGps extends GpsInterface {
-  final _latitudeCorrector = ErrorCorrector(maxSamples: 1, maxDeviation: GpsInterface.gpsError * 10);
-  final _longitudeCorrector = ErrorCorrector(maxSamples: 1, maxDeviation: GpsInterface.gpsError * 10);
-  RoverGps({required super.collection});
-
-  @override
-  Future<bool> init() async {
-    collection.server.messages.onMessage(
-      name: RoverPosition().messageName,
-      constructor: RoverPosition.fromBuffer,
-      callback: (pos) {
-        if (pos.hasGps()) update(pos.gps);
-      },
-    );
-    return super.init();
-  }
-
-  @override
-  Future<void> dispose() async {
-    _latitudeCorrector.clear();
-    _longitudeCorrector.clear();
-  }
-
-	@override
-  void update(GpsCoordinates newValue) {
-    _latitudeCorrector.addValue(newValue.latitude);
-    _longitudeCorrector.addValue(newValue.longitude);
-    hasValue = true;
-  }
-
-  @override
- GpsCoordinates get coordinates => GpsCoordinates(
-   latitude: _latitudeCorrector.calibratedValue,
-   longitude: _longitudeCorrector.calibratedValue,
- );
-}
diff --git a/lib/src/rover/imu.dart b/lib/src/rover/imu.dart
deleted file mode 100644
index 8428ae5..0000000
--- a/lib/src/rover/imu.dart
+++ /dev/null
@@ -1,44 +0,0 @@
-import "package:burt_network/burt_network.dart";
-import "package:autonomy/interfaces.dart";
-
-import "corrector.dart";
-
-class RoverImu extends ImuInterface {
-  final _zCorrector = ErrorCorrector(maxSamples: 10, maxDeviation: 15);
-  RoverImu({required super.collection});
-
-	Orientation value = Orientation();
-
-  @override
-  Future<bool> init() async {
-    collection.server.messages.onMessage(
-      name: RoverPosition().messageName,
-      constructor: RoverPosition.fromBuffer,
-      callback: (pos) {
-        if (pos.hasOrientation()) update(pos.orientation);
-      },
-    );
-    return super.init();
-  }
-
-  @override
-  Future<void> dispose() async {
-    _zCorrector.clear();
-  }
-
-  @override
-  void update(Orientation newValue) {
-    //  _zCorrector.addValue(newValue.heading);
-    //	collection.logger.trace("Got IMU value");
-    print("Got imu: ${newValue.heading}. Direction: ${collection.drive.orientation}");
-    hasValue = true;
-    value = newValue;
-	}
-
-  @override
-  Orientation get raw => Orientation(
-    x: 0,
-    y: 0,
-    z: value.z,
-  ).clampHeading();
-}
diff --git a/lib/src/rover/sensorless.dart b/lib/src/rover/sensorless.dart
deleted file mode 100644
index 7a9b7b4..0000000
--- a/lib/src/rover/sensorless.dart
+++ /dev/null
@@ -1,76 +0,0 @@
-import "package:autonomy/interfaces.dart";
-import "package:autonomy/rover.dart";
-import "package:autonomy/simulator.dart";
-
-class SensorlessDrive extends DriveInterface {
-  final DriveInterface simulatedDrive;
-  final DriveInterface realDrive;
-  
-  SensorlessDrive({required super.collection, bool useGps = true, bool useImu = true}) : 
-    simulatedDrive = DriveSimulator(collection: collection),
-    realDrive = RoverDrive(collection: collection, useGps: useGps, useImu: useImu);
-
-  @override
-  Future<bool> init() async {
-    var result = true;
-    result &= await simulatedDrive.init();
-    result &= await realDrive.init();
-    return result;
-  }
-
-  @override
-  Future<void> dispose() async {
-    await simulatedDrive.dispose();
-    await realDrive.dispose();
-  }
-
-  @override
-  Future<void> goForward() async {
-    await simulatedDrive.goForward();
-    await realDrive.goForward();
-  }
-
-  @override
-  Future<void> stop() async {
-    await simulatedDrive.stop();
-    await realDrive.stop();
-  }
-
-  @override
-  Future<void> faceNorth() async {
-    await simulatedDrive.faceNorth();
-    await realDrive.faceNorth();
-  }
-
-
-  @override
-  Future<void> faceDirection(DriveOrientation orientation) async {
-    await simulatedDrive.faceDirection(orientation);
-    await realDrive.faceDirection(orientation);
-    await super.faceDirection(orientation);
-  }
-
-  @override
-  Future<void> turnLeft() async {
-    await simulatedDrive.turnLeft();
-    await realDrive.turnLeft();
-  }
-
-  @override
-  Future<void> turnRight() async {
-    await simulatedDrive.turnRight();
-    await realDrive.turnRight();
-  }
-
-  @override
-  Future<void> turnQuarterLeft() async {
-    await simulatedDrive.turnQuarterLeft();
-    await realDrive.turnQuarterLeft();
-  }
-
-  @override
-  Future<void> turnQuarterRight() async {
-    await simulatedDrive.turnQuarterRight();
-    await realDrive.turnQuarterRight();
-  }
-}
diff --git a/lib/src/simulator/drive.dart b/lib/src/simulator/drive.dart
deleted file mode 100644
index b1b04d8..0000000
--- a/lib/src/simulator/drive.dart
+++ /dev/null
@@ -1,73 +0,0 @@
-// ignore_for_file: cascade_invocations
-
-import "package:autonomy/interfaces.dart";
-import "package:burt_network/generated.dart";
-
-class DriveSimulator extends DriveInterface {
-  final bool shouldDelay;
-  DriveSimulator({required super.collection, this.shouldDelay = false});
-
-  @override
-  Future<bool> init() async => true;
-
-  @override
-  Future<void> dispose() async { }
-
-  @override
-  Future<void> goForward() async {
-    final position = collection.gps.coordinates;
-    final orientation = collection.imu.orientation;
-    final newPosition = position.goForward(orientation!);
-    collection.logger.debug("Going forward");
-    collection.logger.trace("  Old position: ${position.prettyPrint()}");
-    collection.logger.trace("  Orientation: $orientation");
-    collection.logger.trace("  New position: ${newPosition.prettyPrint()}");
-    collection.gps.update(newPosition);
-    if (shouldDelay) await Future<void>.delayed(const Duration(milliseconds: 500));
-  }
-
-  @override
-  Future<void> faceNorth() async {
-    collection.imu.update(OrientationUtils.north);
-    if (shouldDelay) await Future<void>.delayed(const Duration(milliseconds: 500));
-  }
-
-  @override
-  Future<void> turnLeft() async  {
-    collection.logger.debug("Turning left");
-    final heading = collection.imu.heading;
-    final orientation = Orientation(z: heading + 90);
-    collection.imu.update(orientation);
-    if (shouldDelay) await Future<void>.delayed(const Duration(milliseconds: 500));
-  }
-
-  @override
-  Future<void> turnRight() async  {
-    collection.logger.debug("Turning right");
-    final heading = collection.imu.heading;
-    final orientation = Orientation(z: heading - 90);
-    collection.imu.update(orientation);
-    if (shouldDelay) await Future<void>.delayed(const Duration(milliseconds: 500));
-  }
-
-  @override
-  Future<void> turnQuarterLeft() async {
-    collection.logger.debug("Turning quarter left");
-    final heading = collection.imu.heading;
-    final orientation = Orientation(z: heading + 45).clampHeading();
-    collection.imu.update(orientation);
-    if (shouldDelay) await Future<void>.delayed(const Duration(milliseconds: 500));
-  }
-
-  @override
-  Future<void> turnQuarterRight() async {
-    collection.logger.debug("Turning quarter right");
-    final heading = collection.imu.heading;
-    final orientation = Orientation(z: heading - 45).clampHeading();
-    collection.imu.update(orientation);
-    if (shouldDelay) await Future<void>.delayed(const Duration(milliseconds: 500));
-  }
-
-  @override
-  Future<void> stop() async => collection.logger.debug("Stopping");
-}
diff --git a/lib/src/simulator/pathfinding.dart b/lib/src/simulator/pathfinding.dart
deleted file mode 100644
index bce45b6..0000000
--- a/lib/src/simulator/pathfinding.dart
+++ /dev/null
@@ -1,25 +0,0 @@
-import "package:autonomy/interfaces.dart";
-import "package:burt_network/generated.dart";
-
-class PathfindingSimulator extends PathfindingInterface {  
-  static int i = 0;
-  
-  PathfindingSimulator({required super.collection});
-
-  @override
-  Future<bool> init() async => true;
-
-  @override
-  List<AutonomyAStarState> getPath(GpsCoordinates destination, {bool verbose = false}) => [
-    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 0).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.right, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 0).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 1).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.forward, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 2).toGps(), orientation: DriveOrientation.east, direction: DriveDirection.left, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 0, long: 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 1, long: 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 2).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.left, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 2).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.forward, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 1).toGps(), orientation: DriveOrientation.west, direction: DriveDirection.right, collection: collection),
-    AutonomyAStarState(depth: i++, goal: (lat: 2, long: 1).toGps(), position: (lat: 2, long: 1).toGps(), orientation: DriveOrientation.north, direction: DriveDirection.forward, collection: collection),
-  ];
-}
diff --git a/lib/src/utils/a_star.dart b/lib/src/utils/a_star.dart
new file mode 100644
index 0000000..add8e17
--- /dev/null
+++ b/lib/src/utils/a_star.dart
@@ -0,0 +1,184 @@
+import "dart:math";
+
+import "package:a_star/a_star.dart";
+
+import "package:autonomy/interfaces.dart";
+
+class AutonomyAStarState extends AStarState<AutonomyAStarState> {
+  static double getCost(DriveDirection direction) {
+    if (direction == DriveDirection.forward) {
+      return 1;
+    } else if (direction == DriveDirection.quarterLeft || direction == DriveDirection.quarterRight) {
+      return sqrt2;
+    } else {
+      return 2 * sqrt2;
+    }
+  }
+
+  final DriveDirection instruction;
+  final GpsCoordinates position;
+  final CardinalDirection orientation;
+  final GpsCoordinates goal;
+  final AutonomyInterface collection;
+
+  AutonomyAStarState({
+    required this.position,
+    required this.goal,
+    required this.collection,
+    required this.instruction,
+    required this.orientation,
+    required super.depth,
+  });
+
+  factory AutonomyAStarState.start({
+    required AutonomyInterface collection,
+    required GpsCoordinates goal,
+  }) => AutonomyAStarState(
+    position: collection.gps.coordinates,
+    goal: goal,
+    collection: collection,
+    instruction: DriveDirection.stop,
+    orientation: collection.imu.nearest,
+    depth: 0,
+  );
+
+  AutonomyAStarState copyWith({
+    required DriveDirection direction,
+    required CardinalDirection orientation,
+    required GpsCoordinates position,
+  }) => AutonomyAStarState(
+    collection: collection,
+    position: position,
+    orientation: orientation,
+    instruction: direction,
+    goal: goal,
+    depth: depth + getCost(direction),
+  );
+
+  @override
+  String toString() => switch(instruction) {
+    DriveDirection.forward => "Go forward to ${position.prettyPrint()}",
+    DriveDirection.left => "Turn left to face $instruction",
+    DriveDirection.right => "Turn right to face $instruction",
+    DriveDirection.stop => "Start/Stop at ${position.prettyPrint()}",
+    DriveDirection.quarterLeft => "Turn 45 degrees left to face $instruction",
+    DriveDirection.quarterRight => "Turn 45 degrees right to face $instruction",
+  };
+
+  @override
+  double heuristic() => position.distanceTo(goal);
+
+  @override
+  String hash() => "${position.prettyPrint()} ($orientation)";
+
+  @override
+  bool isGoal() => position.isNear(goal, min(GpsUtils.moveLengthMeters, GpsUtils.maxErrorMeters));
+
+  /// Returns whether or not the rover will drive between or right next to an obstacle diagonally<br/>
+  /// <br/>
+  /// Case 1:<br/>
+  /// 0 X<br/>
+  /// X R<br/>
+  /// Assuming the rover is facing 0 and trying to drive forward, will return false<br/>
+  /// <br/>
+  /// Case 2:<br/>
+  /// 0 X<br/>
+  /// X R<br/>
+  /// Assuming the rover is facing north and trying to turn 45 degrees left, will return false<br/>
+  /// <br/>
+  /// Case 3:<br/>
+  /// 0 X<br/>
+  /// 0 R<br/>
+  /// If the rover is facing left but trying to turn 45 degrees right, will return false<br/>
+  /// <br/>
+  /// Case 4:<br/>
+  /// 0 X 0<br/>
+  /// 0 R 0<br/>
+  /// If the rover is facing northeast to 0 and trying to turn left, will return false
+  bool willDriveThroughObstacle(AutonomyAStarState state) {
+    final isTurn = state.instruction != DriveDirection.forward;
+    final isQuarterTurn = state.instruction == DriveDirection.quarterLeft || state.instruction == DriveDirection.quarterRight;
+
+    if (
+      // Can't hit an obstacle while turning
+      state.instruction != DriveDirection.forward
+
+      // Forward drive across the perpendicular axis
+      || (!isTurn && state.orientation.isPerpendicular)
+
+      // Not encountering any sort of diagonal angle
+      || (isTurn && isQuarterTurn && state.orientation.isPerpendicular)
+
+      // No diagonal movement, won't drive between obstacles
+      || (!isQuarterTurn && orientation.isPerpendicular)
+    ) {
+      return false;
+    }
+
+    final CardinalDirection orientation1;
+    final CardinalDirection orientation2;
+
+    // Case 1, trying to drive while facing a 45 degree angle
+    if (!isTurn) {
+      orientation1 = state.orientation.turnQuarterLeft();
+      orientation2 = state.orientation.turnQuarterRight();
+    } else if (isQuarterTurn) { // Case 2 and Case 3
+      orientation1 = orientation;
+      orientation2 = (state.instruction == DriveDirection.quarterLeft)
+        ? orientation1.turnLeft()
+        : orientation1.turnRight();
+    } else { // Case 4
+      orientation1 = (state.instruction == DriveDirection.left)
+        ? orientation.turnQuarterLeft()
+        : orientation.turnQuarterRight();
+      orientation2 = (state.instruction == DriveDirection.left)
+        ? state.orientation.turnQuarterLeft()
+        : state.orientation.turnQuarterRight();
+    }
+
+    // Since the state being passed has a position of moving after the
+    // turn, we have to check the position of where it started
+    return collection.pathfinder.isObstacle(position.goForward(orientation1))
+      || collection.pathfinder.isObstacle(position.goForward(orientation2));
+  }
+
+  bool isValidState(AutonomyAStarState state) =>
+    !collection.pathfinder.isObstacle(state.position)
+    && !willDriveThroughObstacle(state);
+
+  Iterable<AutonomyAStarState> _allNeighbors() => [
+    copyWith(
+      direction: DriveDirection.forward,
+      orientation: orientation,
+      position: position.goForward(orientation),
+    ),
+    copyWith(
+      direction: DriveDirection.left,
+      orientation: orientation.turnLeft(),
+      position: position,
+    ),
+    copyWith(
+      direction: DriveDirection.right,
+      orientation: orientation.turnRight(),
+      position: position,
+    ),
+    copyWith(
+      direction: DriveDirection.quarterLeft,
+      orientation: orientation.turnQuarterLeft(),
+      position: position,
+    ),
+    copyWith(
+      direction: DriveDirection.quarterRight,
+      orientation: orientation.turnQuarterRight(),
+      position: position,
+    ),
+    copyWith(
+      direction: DriveDirection.stop,
+      orientation: orientation,
+      position: position,
+    ),
+  ];
+
+  @override
+  Iterable<AutonomyAStarState> expand() => _allNeighbors().where(isValidState);
+}
diff --git a/lib/src/rover/corrector.dart b/lib/src/utils/corrector.dart
similarity index 88%
rename from lib/src/rover/corrector.dart
rename to lib/src/utils/corrector.dart
index 46b59e1..7339f02 100644
--- a/lib/src/rover/corrector.dart
+++ b/lib/src/utils/corrector.dart
@@ -1,13 +1,14 @@
 import "dart:collection";
 
-class ErrorCorrector {  // non-nullable
+class ErrorCorrector {
   final int maxSamples;
   final double maxDeviation;
   ErrorCorrector({required this.maxSamples, this.maxDeviation = double.infinity});
-  
+  factory ErrorCorrector.disabled() => ErrorCorrector(maxSamples: 1);
+
   double calibratedValue = 0;
   final Queue<double> recentSamples = DoubleLinkedQueue();
-  
+
   void addValue(double value) {
     if (recentSamples.isEmpty) {
       recentSamples.add(value);
@@ -32,7 +33,7 @@ extension on Iterable<num> {
     num sum = 0;
     var count = 0;
     for (final element in this) {
-      sum += element; 
+      sum += element;
       count++;
     }
     return sum / count;
diff --git a/lib/src/interfaces/error.dart b/lib/src/utils/error.dart
similarity index 100%
rename from lib/src/interfaces/error.dart
rename to lib/src/utils/error.dart
diff --git a/lib/src/interfaces/gps_utils.dart b/lib/src/utils/gps_utils.dart
similarity index 72%
rename from lib/src/interfaces/gps_utils.dart
rename to lib/src/utils/gps_utils.dart
index dc6c19e..7e56f0e 100644
--- a/lib/src/interfaces/gps_utils.dart
+++ b/lib/src/utils/gps_utils.dart
@@ -2,15 +2,14 @@
 import "dart:math";
 
 import "package:autonomy/interfaces.dart";
-import "package:burt_network/generated.dart";
 
 extension GpsUtils on GpsCoordinates {
-  static double maxErrorMeters = 1;
+  static double maxErrorMeters = 0.5;
   static double moveLengthMeters = 1;
-  static double get epsilonLatitude => maxErrorMeters * latitudePerMeter; 
+  static double get epsilonLatitude => maxErrorMeters * latitudePerMeter;
   static double get epsilonLongitude => maxErrorMeters * longitudePerMeter;
 
-  static double get movementLatitude => moveLengthMeters * latitudePerMeter; 
+  static double get movementLatitude => moveLengthMeters * latitudePerMeter;
   static double get movementLongitude => moveLengthMeters * longitudePerMeter;
 
   static GpsCoordinates get east => GpsCoordinates(longitude: -movementLongitude);
@@ -24,20 +23,19 @@ extension GpsUtils on GpsCoordinates {
 
   // Taken from https://stackoverflow.com/a/39540339/9392211
   static const metersPerLatitude = 111.32 * 1000;  // 111.32 km
-  // static const metersPerLatitude = 1;
   static const radiansPerDegree = pi / 180;
   static double get metersPerLongitude => 40075 * cos(GpsInterface.currentLatitude * radiansPerDegree) / 360 * 1000.0;
-  
+
   static double get latitudePerMeter => 1 / metersPerLatitude;
   static double get longitudePerMeter => 1 / metersPerLongitude;
-  
+
   double distanceTo(GpsCoordinates other) => sqrt(
     pow(latitude - other.latitude, 2) +
     pow(longitude - other.longitude, 2),
   );
 
-  double manhattanDistance(GpsCoordinates other) => 
-    (latitude - other.latitude).abs() * metersPerLatitude + 
+  double manhattanDistance(GpsCoordinates other) =>
+    (latitude - other.latitude).abs() * metersPerLatitude +
     (longitude - other.longitude).abs() * metersPerLongitude;
 
   bool isNear(GpsCoordinates other, [double? tolerance]) {
@@ -60,17 +58,16 @@ extension GpsUtils on GpsCoordinates {
     longitude: longitude + other.longitude,
   );
 
-//  String prettyPrint() => "(lat=${(latitude * GpsUtils.metersPerLatitude).toStringAsFixed(2)}, long=${(longitude * GpsUtils.metersPerLongitude).toStringAsFixed(2)})";
   String prettyPrint() => toProto3Json().toString();
 
-  GpsCoordinates goForward(DriveOrientation orientation) => this + switch(orientation) {
-    DriveOrientation.north => GpsUtils.north,
-    DriveOrientation.south => GpsUtils.south,
-    DriveOrientation.west => GpsUtils.west,
-    DriveOrientation.east => GpsUtils.east,
-    DriveOrientation.northEast => GpsUtils.northEast,
-    DriveOrientation.northWest => GpsUtils.northWest,
-    DriveOrientation.southEast => GpsUtils.southEast,
-    DriveOrientation.southWest => GpsUtils.southWest,
+  GpsCoordinates goForward(CardinalDirection orientation) => this + switch(orientation) {
+    CardinalDirection.north => GpsUtils.north,
+    CardinalDirection.south => GpsUtils.south,
+    CardinalDirection.west => GpsUtils.west,
+    CardinalDirection.east => GpsUtils.east,
+    CardinalDirection.northEast => GpsUtils.northEast,
+    CardinalDirection.northWest => GpsUtils.northWest,
+    CardinalDirection.southEast => GpsUtils.southEast,
+    CardinalDirection.southWest => GpsUtils.southWest,
   };
 }
diff --git a/lib/src/utils/imu_utils.dart b/lib/src/utils/imu_utils.dart
new file mode 100644
index 0000000..5a45f9a
--- /dev/null
+++ b/lib/src/utils/imu_utils.dart
@@ -0,0 +1,29 @@
+import "package:burt_network/protobuf.dart";
+
+extension OrientationUtils on Orientation {
+  static const double epsilon = 3.5;
+  static const double orientationEpsilon = 10;
+
+  static final north = Orientation(z: 0);
+  static final west = Orientation(z: 90);
+  static final south = Orientation(z: 180);
+  static final east = Orientation(z: 270);
+
+  double get heading => z;
+
+  bool get isEmpty => x == 0 && y == 0 && z == 0;
+
+  bool isNear(double value) {
+    if (value > 270 && z < 90) {
+      return (z + 360 - value).abs() < epsilon;
+    } else if (value < 90 && z > 270) {
+      return (z - value - 360).abs() < epsilon;
+    } else {
+      return (z - value).abs() < epsilon;
+    }
+  }
+}
+
+extension AngleUtils on double {
+  double clampAngle() => ((this % 360) + 360) % 360;
+}
diff --git a/lib/src/utils/receiver.dart b/lib/src/utils/receiver.dart
new file mode 100644
index 0000000..d301496
--- /dev/null
+++ b/lib/src/utils/receiver.dart
@@ -0,0 +1,21 @@
+import "dart:async";
+
+mixin Receiver {
+  Completer<void>? _completer;
+
+  bool _hasValue = false;
+
+  set hasValue(bool value) {
+    _hasValue = value;
+    if (!value) return;
+    _completer?.complete();
+    _completer = null;
+  }
+
+  bool get hasValue => _hasValue;
+
+  Future<void> waitForValue() {
+    _completer = Completer<bool>();
+    return _completer!.future;
+  }
+}
diff --git a/lib/src/interfaces/reporter.dart b/lib/src/utils/reporter.dart
similarity index 86%
rename from lib/src/interfaces/reporter.dart
rename to lib/src/utils/reporter.dart
index 05e848b..744eb80 100644
--- a/lib/src/interfaces/reporter.dart
+++ b/lib/src/utils/reporter.dart
@@ -1,5 +1,4 @@
 import "dart:async";
-import "package:burt_network/generated.dart";
 
 import "package:autonomy/interfaces.dart";
 
@@ -13,7 +12,7 @@ mixin ValueReporter on Service {
   @override
   Future<bool> init() async {
     timer = Timer.periodic(reportInterval, (timer) => _reportValue());
-    return await super.init();
+    return super.init();
   }
 
   @override
diff --git a/lib/src/rover/video.dart b/lib/src/video/rover_video.dart
similarity index 67%
rename from lib/src/rover/video.dart
rename to lib/src/video/rover_video.dart
index 2296331..b9442d2 100644
--- a/lib/src/rover/video.dart
+++ b/lib/src/video/rover_video.dart
@@ -1,7 +1,6 @@
 import "dart:async";
 
 import "package:autonomy/interfaces.dart";
-import "package:burt_network/burt_network.dart";
 
 class RoverVideo extends VideoInterface {
   RoverVideo({required super.collection});
@@ -22,11 +21,11 @@ class RoverVideo extends VideoInterface {
   @override
   void updateFrame(VideoData newData) {
     data = newData;
-    if (data.arucoDetected == BoolState.YES) {
-      flag = true;
-      Timer(const Duration(seconds: 3), () => flag = false);
-      collection.logger.info("Is ArUco detected: ${data.arucoDetected}");
-    }
+    // if (data.arucoDetected == BoolState.YES) {
+    //   flag = true;
+    //   Timer(const Duration(seconds: 3), () => flag = false);
+    //   collection.logger.info("Is ArUco detected: ${data.arucoDetected}");
+    // }
     hasValue  = true;
   }
 }
diff --git a/lib/src/simulator/realsense.dart b/lib/src/video/sim_video.dart
similarity index 89%
rename from lib/src/simulator/realsense.dart
rename to lib/src/video/sim_video.dart
index 0e20191..c7d5f1c 100644
--- a/lib/src/simulator/realsense.dart
+++ b/lib/src/video/sim_video.dart
@@ -1,11 +1,10 @@
 import "dart:typed_data";
 
 import "package:autonomy/interfaces.dart";
-import "package:burt_network/generated.dart";
 
 class VideoSimulator extends VideoInterface {
   VideoSimulator({required super.collection});
-  
+
   @override
   Future<bool> init() async {
     hasValue = true;
@@ -14,7 +13,7 @@ class VideoSimulator extends VideoInterface {
 
   @override
   Future<void> dispose() async => depthFrame = Uint16List.fromList([]);
-  
+
   Uint16List depthFrame = Uint16List.fromList([]);
 
   @override
diff --git a/lib/src/interfaces/video.dart b/lib/src/video/video_interface.dart
similarity index 71%
rename from lib/src/interfaces/video.dart
rename to lib/src/video/video_interface.dart
index e66dc3e..ed76473 100644
--- a/lib/src/interfaces/video.dart
+++ b/lib/src/video/video_interface.dart
@@ -1,5 +1,4 @@
 import "package:autonomy/interfaces.dart";
-import "package:burt_network/generated.dart";
 
 /// Handles obstacle detection data and ArUco data from video
 abstract class VideoInterface extends Service with Receiver {
@@ -12,6 +11,6 @@ bool flag = false;
 
   void updateFrame(VideoData newData);
 
-  double get arucoSize => data.arucoSize;
-  double get arucoPosition => data.arucoPosition;
+  double get arucoSize => 0; //data.arucoSize;
+  double get arucoPosition => 0; //data.arucoPosition;
 }
diff --git a/lib/utils.dart b/lib/utils.dart
new file mode 100644
index 0000000..4829620
--- /dev/null
+++ b/lib/utils.dart
@@ -0,0 +1,9 @@
+export "src/utils/a_star.dart";
+export "src/utils/corrector.dart";
+export "src/utils/error.dart";
+export "src/utils/gps_utils.dart";
+export "src/utils/imu_utils.dart";
+export "src/utils/receiver.dart";
+export "src/utils/reporter.dart";
+
+export "package:burt_network/burt_network.dart" show Service;
diff --git a/pubspec.lock b/pubspec.lock
index 69567f5..f67921e 100644
--- a/pubspec.lock
+++ b/pubspec.lock
@@ -57,12 +57,10 @@ packages:
   burt_network:
     dependency: "direct main"
     description:
-      path: "."
-      ref: "2.2.0"
-      resolved-ref: "7ec80052bd9d2777782f7ce44cfcfc1e69e8a582"
-      url: "https://github.com/BinghamtonRover/Networking.git"
-    source: git
-    version: "2.2.0"
+      path: "../../burt_network"
+      relative: true
+    source: path
+    version: "2.3.0"
   collection:
     dependency: transitive
     description:
@@ -239,15 +237,6 @@ packages:
       url: "https://pub.dev"
     source: hosted
     version: "2.0.2"
-  opencv_ffi:
-    dependency: "direct main"
-    description:
-      path: "."
-      ref: HEAD
-      resolved-ref: fb721ce518faa62b470c73ae58edfe825a5f9052
-      url: "https://github.com/BinghamtonRover/OpenCV-FFI.git"
-    source: git
-    version: "1.2.0"
   package_config:
     dependency: transitive
     description:
diff --git a/pubspec.yaml b/pubspec.yaml
index daf64d3..841baf4 100644
--- a/pubspec.yaml
+++ b/pubspec.yaml
@@ -8,12 +8,10 @@ environment:
 
 # Try not to edit this section by hand. Use `dart pub add my_package`
 dependencies:
-  opencv_ffi:
-    git: https://github.com/BinghamtonRover/OpenCV-FFI.git
   burt_network:
-    git: 
+    git:
       url: https://github.com/BinghamtonRover/Networking.git
-      ref: 2.2.0
+      ref: 2.3.0
   a_star: ^3.0.0
   meta: ^1.11.0
 
diff --git a/test/network_test.dart b/test/network_test.dart
index dc5a1fd..f6595f5 100644
--- a/test/network_test.dart
+++ b/test/network_test.dart
@@ -106,9 +106,6 @@ void main() => group("[Network]", tags: ["network"], () {
     simulator.drive = SensorlessDrive(collection: simulator, useGps: false, useImu: false);
     await simulator.init();
 
-    await simulator.drive.faceNorth();
-    expect(simulator.imu.orientation, DriveOrientation.north);
-
     final origin = GpsCoordinates(latitude: 0, longitude: 0);
     final oneMeter = (lat: 1, long: 0).toGps();
     expect(subsystems.throttle, 0);
diff --git a/test/orchestrator_test.dart b/test/orchestrator_test.dart
index 7144f89..82084eb 100644
--- a/test/orchestrator_test.dart
+++ b/test/orchestrator_test.dart
@@ -1,4 +1,3 @@
-import "package:autonomy/src/rover/gps.dart";
 import "package:test/test.dart";
 import "package:autonomy/autonomy.dart";
 import "package:burt_network/burt_network.dart";
diff --git a/test/path_test.dart b/test/path_test.dart
index 9f43d99..f246b74 100644
--- a/test/path_test.dart
+++ b/test/path_test.dart
@@ -77,7 +77,7 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
     expect(path, isNotEmpty);
     for (final step in path) {
       simulator.logger.trace(step.toString());
-      expect(simulator.pathfinder.isObstacle(step.position), isFalse);
+      expect(simulator.pathfinder.isObstacle(step.endPosition), isFalse);
     }
     expect(path.length, 10, reason: "1 turn + 1 forward + 1 turn + 4 forward + 1 45 degree turn + 1 forward + 1 stop = 10 steps total");
     await simulator.drive.followPath(path);
diff --git a/test/rover_test.dart b/test/rover_test.dart
index 2fb4bbb..9621d32 100644
--- a/test/rover_test.dart
+++ b/test/rover_test.dart
@@ -1,6 +1,6 @@
 import "package:test/test.dart";
 
-import "package:burt_network/generated.dart";
+import "package:burt_network/protobuf.dart";
 import "package:burt_network/logging.dart";
 
 import "package:autonomy/interfaces.dart";
@@ -8,7 +8,7 @@ import "package:autonomy/rover.dart";
 import "package:autonomy/simulator.dart";
 
 void main() => group("[Rover]", tags: ["rover"], () {
-  test("Can be restarted", () async { 
+  test("Can be restarted", () async {
     Logger.level = LogLevel.off;
     final rover = RoverAutonomy();
     await rover.init();
@@ -16,7 +16,7 @@ void main() => group("[Rover]", tags: ["rover"], () {
     await rover.dispose();
   });
 
-  test("Real pathfinding is coherent", () async { 
+  test("Real pathfinding is coherent", () async {
     Logger.level = LogLevel.off;
     final simulator = AutonomySimulator();
     simulator.pathfinder = RoverPathfinder(collection: simulator);
@@ -69,10 +69,10 @@ Future<void> testPath(AutonomyInterface simulator) async {
     simulator.logger.trace("  From: ${simulator.gps.coordinates.prettyPrint()} facing ${simulator.imu.heading}");
     simulator.logger.debug("  $transition");
     await simulator.drive.goDirection(transition.direction);
-    expect(simulator.gps.isNear(transition.position), isTrue);
+    expect(simulator.gps.isNear(transition.endPosition), isTrue);
     simulator.logger.trace("New orientation: ${simulator.imu.heading}");
-    simulator.logger.trace("Expected orientation: ${transition.orientation}");
-    expect(simulator.imu.orientation, transition.orientation);
+    simulator.logger.trace("Expected orientation: ${transition.endOrientation}");
+    expect(simulator.imu.orientation, transition.endOrientation);
   }
 }
 
@@ -92,9 +92,9 @@ Future<void> testPath2(AutonomyInterface simulator) async {
     simulator.logger.debug(transition.toString());
     simulator.logger.trace("  From: ${simulator.gps.coordinates.prettyPrint()}");
     await simulator.drive.goDirection(transition.direction);
-    expect(simulator.gps.isNear(transition.position), isTrue);
+    expect(simulator.gps.isNear(transition.endPosition), isTrue);
     expect(simulator.pathfinder.isObstacle(simulator.gps.coordinates), isFalse);
-    expect(simulator.imu.orientation, transition.orientation);
+    expect(simulator.imu.orientation, transition.endOrientation);
     simulator.logger.trace("  To: ${simulator.gps.coordinates.prettyPrint()}");
   }
 }
diff --git a/test/sensor_test.dart b/test/sensor_test.dart
index 017662a..dce2233 100644
--- a/test/sensor_test.dart
+++ b/test/sensor_test.dart
@@ -1,12 +1,10 @@
-import "package:autonomy/src/rover/imu.dart";
-import "package:burt_network/logging.dart";
 import "package:test/test.dart";
 
-import "package:burt_network/generated.dart";
+import "package:burt_network/protobuf.dart";
+import "package:burt_network/logging.dart";
 
 import "package:autonomy/interfaces.dart";
 import "package:autonomy/simulator.dart";
-import "package:autonomy/src/rover/gps.dart";
 
 const imuError = 2.5;
 const gpsPrecision = 7;
@@ -46,7 +44,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     final oldError = GpsUtils.maxErrorMeters;
     GpsUtils.maxErrorMeters = 3;
     Logger.level = LogLevel.off;
-    
+
     final simulator = AutonomySimulator();
     final realGps = RoverGps(collection: simulator);
     final simulatedGps = GpsSimulator(collection: simulator, maxError: GpsInterface.gpsError);
@@ -84,7 +82,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     final realImu = RoverImu(collection: simulator);
     final north = OrientationUtils.north;
     simulatedImu.update(north);
-    
+
     var count = 0;
     for (var i = 0; i < 1000; i++) {
       final newData = simulatedImu.raw;
@@ -101,7 +99,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     simulator.logger.info("Final orientation: ${realImu.heading}");
     expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage");
 
-    
+
     realImu.update(OrientationUtils.south);
     expect(realImu.isNear(OrientationUtils.north.heading), isTrue);
     await simulator.dispose();
@@ -149,7 +147,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     final realImu = RoverImu(collection: simulator);
     final orientation = OrientationUtils.north;
     simulatedImu.update(orientation);
-    
+
     var count = 0;
     for (var i = 0; i < 350; i++) {
       orientation.z += 1;
@@ -184,7 +182,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     simulator.gps.update(utah);
     expect(simulator.hasValue, isFalse);
     expect(GpsInterface.currentLatitude, 0);
-    
+
     await simulator.init();
     await simulator.waitForValue();
     expect(simulator.hasValue, isTrue);
@@ -219,7 +217,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     final realImu = RoverImu(collection: simulator);
     final orientation = Orientation(z: 360);
     simulatedImu.update(orientation);
-    
+
     var count = 0;
     for (var i = 0; i < 1000; i++) {
       final newData = simulatedImu.raw;

From b0655a55d7bf1934ebc7299d846cfda9312578c7 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Sat, 14 Dec 2024 00:28:56 -0500
Subject: [PATCH 13/21] Corrected directions

---
 lib/src/imu/cardinal_direction.dart |  8 ++++----
 lib/src/utils/gps_utils.dart        | 12 ++++++------
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/lib/src/imu/cardinal_direction.dart b/lib/src/imu/cardinal_direction.dart
index 40b2521..410e456 100644
--- a/lib/src/imu/cardinal_direction.dart
+++ b/lib/src/imu/cardinal_direction.dart
@@ -6,10 +6,10 @@ enum CardinalDirection {
   west(90),
   south(180),
   east(270),
-  northEast(000 + 45),
-  northWest(360 - 45),
-  southWest(180 + 45),
-  southEast(180 - 45);
+  northEast(0 - 45),
+  northWest(0 + 45),
+  southWest(180 - 45),
+  southEast(180 + 45);
 
   final double angle;
   const CardinalDirection(this.angle);
diff --git a/lib/src/utils/gps_utils.dart b/lib/src/utils/gps_utils.dart
index 7e56f0e..ac49d42 100644
--- a/lib/src/utils/gps_utils.dart
+++ b/lib/src/utils/gps_utils.dart
@@ -12,14 +12,14 @@ extension GpsUtils on GpsCoordinates {
   static double get movementLatitude => moveLengthMeters * latitudePerMeter;
   static double get movementLongitude => moveLengthMeters * longitudePerMeter;
 
-  static GpsCoordinates get east => GpsCoordinates(longitude: -movementLongitude);
-  static GpsCoordinates get west => GpsCoordinates(longitude: movementLongitude);
+  static GpsCoordinates get east => GpsCoordinates(longitude: movementLongitude);
+  static GpsCoordinates get west => GpsCoordinates(longitude: -movementLongitude);
   static GpsCoordinates get north => GpsCoordinates(latitude: movementLatitude);
   static GpsCoordinates get south => GpsCoordinates(latitude: -movementLatitude);
-  static GpsCoordinates get northEast => GpsCoordinates(latitude: movementLatitude, longitude: -movementLongitude);
-  static GpsCoordinates get northWest => GpsCoordinates(latitude: movementLatitude, longitude: movementLongitude);
-  static GpsCoordinates get southEast => GpsCoordinates(latitude: -movementLatitude, longitude: -movementLongitude);
-  static GpsCoordinates get southWest => GpsCoordinates(latitude: -movementLatitude, longitude: movementLongitude);
+  static GpsCoordinates get northEast => GpsCoordinates(latitude: movementLatitude, longitude: movementLongitude);
+  static GpsCoordinates get northWest => GpsCoordinates(latitude: movementLatitude, longitude: -movementLongitude);
+  static GpsCoordinates get southEast => GpsCoordinates(latitude: -movementLatitude, longitude: movementLongitude);
+  static GpsCoordinates get southWest => GpsCoordinates(latitude: -movementLatitude, longitude: -movementLongitude);
 
   // Taken from https://stackoverflow.com/a/39540339/9392211
   static const metersPerLatitude = 111.32 * 1000;  // 111.32 km

From fa81bf839f7b2822e9494b6637aeee013b0a34f8 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Mon, 16 Dec 2024 19:25:48 -0500
Subject: [PATCH 14/21] Initial work on fixing unit tests

---
 lib/src/drive/drive_config.dart    |   2 +-
 lib/src/drive/drive_interface.dart |   5 +-
 lib/src/drive/rover_drive.dart     |   8 +--
 lib/src/drive/sensor_drive.dart    |   2 +-
 lib/src/drive/sim_drive.dart       |   2 +-
 lib/src/drive/timed_drive.dart     |   2 +-
 lib/src/gps/gps_interface.dart     |   4 ++
 lib/src/gps/rover_gps.dart         |   4 ++
 lib/src/imu/imu_interface.dart     |   4 ++
 lib/src/imu/rover_imu.dart         |   4 ++
 lib/src/utils/a_star.dart          |   5 +-
 lib/src/utils/gps_utils.dart       |  25 +++++++
 lib/src/utils/receiver.dart        |  12 ++--
 pubspec.lock                       |   8 ++-
 test/network_test.dart             |  60 ++++++++++------
 test/orchestrator_test.dart        |  14 ++--
 test/path_test.dart                |  15 +++-
 test/rover_test.dart               |  18 ++---
 test/sensor_test.dart              | 106 ++++++++++++++---------------
 19 files changed, 191 insertions(+), 109 deletions(-)

diff --git a/lib/src/drive/drive_config.dart b/lib/src/drive/drive_config.dart
index 2977da6..dfb2919 100644
--- a/lib/src/drive/drive_config.dart
+++ b/lib/src/drive/drive_config.dart
@@ -36,5 +36,5 @@ const tankConfig = DriveConfig(
   turnThrottle: 0.35,
   turnDelay: Duration(milliseconds: 1000),
   oneMeterDelay: Duration(milliseconds: 2000),
-  subsystemsAddress: "localhost",
+  subsystemsAddress: "127.0.0.1",
 );
diff --git a/lib/src/drive/drive_interface.dart b/lib/src/drive/drive_interface.dart
index a1f3951..495851e 100644
--- a/lib/src/drive/drive_interface.dart
+++ b/lib/src/drive/drive_interface.dart
@@ -15,9 +15,8 @@ enum DriveDirection {
 
 abstract class DriveInterface extends Service {
   AutonomyInterface collection;
-  DriveInterface({required this.collection});
-
-  DriveConfig get config => roverConfig;
+  DriveConfig config;
+  DriveInterface({required this.collection, this.config = roverConfig});
 
   Future<void> stop();
 
diff --git a/lib/src/drive/rover_drive.dart b/lib/src/drive/rover_drive.dart
index 434b9a5..60df772 100644
--- a/lib/src/drive/rover_drive.dart
+++ b/lib/src/drive/rover_drive.dart
@@ -10,14 +10,15 @@ class RoverDrive extends DriveInterface {
   final bool useGps;
   final bool useImu;
 
-  late final sensorDrive = SensorDrive(collection: collection);
-  late final timedDrive = TimedDrive(collection: collection);
-  late final simDrive = DriveSimulator(collection: collection);
+  late final sensorDrive = SensorDrive(collection: collection, config: config);
+  late final timedDrive = TimedDrive(collection: collection, config: config);
+  late final simDrive = DriveSimulator(collection: collection, config: config);
 
   RoverDrive({
     required super.collection,
     this.useGps = true,
     this.useImu = true,
+    super.config,
   });
 
   /// Initializes the rover's drive subsystems.
@@ -71,7 +72,6 @@ class RoverDrive extends DriveInterface {
     if (useImu) {
       await sensorDrive.faceDirection(orientation);
     } else {
-      await timedDrive.faceDirection(orientation);
       await simDrive.faceDirection(orientation);
     }
   }
diff --git a/lib/src/drive/sensor_drive.dart b/lib/src/drive/sensor_drive.dart
index 22c03d3..da5c316 100644
--- a/lib/src/drive/sensor_drive.dart
+++ b/lib/src/drive/sensor_drive.dart
@@ -6,7 +6,7 @@ import "drive_commands.dart";
 class SensorDrive extends DriveInterface with RoverDriveCommands {
   static const predicateDelay = Duration(milliseconds: 10);
 
-  SensorDrive({required super.collection});
+  SensorDrive({required super.collection, super.config});
 
   @override
   Future<void> stop() async => stopMotors();
diff --git a/lib/src/drive/sim_drive.dart b/lib/src/drive/sim_drive.dart
index 35d6cb2..a04418f 100644
--- a/lib/src/drive/sim_drive.dart
+++ b/lib/src/drive/sim_drive.dart
@@ -4,7 +4,7 @@ class DriveSimulator extends DriveInterface {
   static const delay = Duration(milliseconds: 500);
 
   final bool shouldDelay;
-  DriveSimulator({required super.collection, this.shouldDelay = false});
+  DriveSimulator({required super.collection, this.shouldDelay = false, super.config});
 
   @override
   Future<bool> init() async => true;
diff --git a/lib/src/drive/timed_drive.dart b/lib/src/drive/timed_drive.dart
index 1ab997a..bf5f2ab 100644
--- a/lib/src/drive/timed_drive.dart
+++ b/lib/src/drive/timed_drive.dart
@@ -5,7 +5,7 @@ import "package:autonomy/interfaces.dart";
 import "drive_commands.dart";
 
 class TimedDrive extends DriveInterface with RoverDriveCommands {
-  TimedDrive({required super.collection});
+  TimedDrive({required super.collection, super.config});
 
   @override
   Future<bool> init() async => true;
diff --git a/lib/src/gps/gps_interface.dart b/lib/src/gps/gps_interface.dart
index e5f8ae7..e4505b6 100644
--- a/lib/src/gps/gps_interface.dart
+++ b/lib/src/gps/gps_interface.dart
@@ -1,4 +1,5 @@
 import "package:autonomy/interfaces.dart";
+import "package:meta/meta.dart";
 
 abstract class GpsInterface extends Service with Receiver {
   static const gpsError = 0.00003;
@@ -11,6 +12,9 @@ abstract class GpsInterface extends Service with Receiver {
   double get latitude => coordinates.latitude;
 
   void update(GpsCoordinates newValue);
+  @visibleForTesting
+  void forceUpdate(GpsCoordinates newValue) {}
+
   GpsCoordinates get coordinates;
 
   bool isNear(GpsCoordinates other) => coordinates.isNear(other);
diff --git a/lib/src/gps/rover_gps.dart b/lib/src/gps/rover_gps.dart
index 80a1cb0..25e60d0 100644
--- a/lib/src/gps/rover_gps.dart
+++ b/lib/src/gps/rover_gps.dart
@@ -26,6 +26,10 @@ class RoverGps extends GpsInterface {
     // Do nothing, since this should only be internally updated
   }
 
+  @override
+  void forceUpdate(GpsCoordinates newValue) =>
+      _internalUpdate(RoverPosition(gps: newValue));
+
   void _internalUpdate(RoverPosition newValue) {
     if (!newValue.hasGps()) return;
     final position = newValue.gps;
diff --git a/lib/src/imu/imu_interface.dart b/lib/src/imu/imu_interface.dart
index 7fbbf83..2bf1d1f 100644
--- a/lib/src/imu/imu_interface.dart
+++ b/lib/src/imu/imu_interface.dart
@@ -1,4 +1,5 @@
 import "package:autonomy/interfaces.dart";
+import "package:meta/meta.dart";
 
 abstract class ImuInterface extends Service with Receiver {
   final AutonomyInterface collection;
@@ -10,6 +11,9 @@ abstract class ImuInterface extends Service with Receiver {
   CardinalDirection get nearest => CardinalDirection.nearest(raw);
 
   void update(Orientation newValue);
+  @visibleForTesting
+  void forceUpdate(Orientation newValue) {}
+
   bool isNear(CardinalDirection direction) => raw.isNear(direction.angle);
 
   @override
diff --git a/lib/src/imu/rover_imu.dart b/lib/src/imu/rover_imu.dart
index 2d9d63a..d9d6728 100644
--- a/lib/src/imu/rover_imu.dart
+++ b/lib/src/imu/rover_imu.dart
@@ -26,6 +26,10 @@ class RoverImu extends ImuInterface {
     // Do nothing, since this should only be internally updated
   }
 
+  @override
+  void forceUpdate(Orientation newValue) =>
+      _internalUpdate(RoverPosition(orientation: newValue));
+
   void _internalUpdate(RoverPosition newValue) {
     if (!newValue.hasOrientation()) return;
     _xCorrector.addValue(newValue.orientation.x);
diff --git a/lib/src/utils/a_star.dart b/lib/src/utils/a_star.dart
index add8e17..530d52d 100644
--- a/lib/src/utils/a_star.dart
+++ b/lib/src/utils/a_star.dart
@@ -6,6 +6,9 @@ import "package:autonomy/interfaces.dart";
 
 class AutonomyAStarState extends AStarState<AutonomyAStarState> {
   static double getCost(DriveDirection direction) {
+    if (direction == DriveDirection.stop) {
+      return 0;
+    }
     if (direction == DriveDirection.forward) {
       return 1;
     } else if (direction == DriveDirection.quarterLeft || direction == DriveDirection.quarterRight) {
@@ -66,7 +69,7 @@ class AutonomyAStarState extends AStarState<AutonomyAStarState> {
   };
 
   @override
-  double heuristic() => position.distanceTo(goal);
+  double heuristic() => position.heuristicDistance(goal);
 
   @override
   String hash() => "${position.prettyPrint()} ($orientation)";
diff --git a/lib/src/utils/gps_utils.dart b/lib/src/utils/gps_utils.dart
index ac49d42..983dcc5 100644
--- a/lib/src/utils/gps_utils.dart
+++ b/lib/src/utils/gps_utils.dart
@@ -34,6 +34,26 @@ extension GpsUtils on GpsCoordinates {
     pow(longitude - other.longitude, 2),
   );
 
+  double heuristicDistance(GpsCoordinates other) {
+    var steps = 0.0;
+    final delta = (this - other).inMeters;
+    final deltaLat = delta.lat.abs();
+    final deltaLong = delta.long.abs();
+
+    final minimumDistance = min(deltaLat, deltaLong);
+    if (minimumDistance >= moveLengthMeters) {
+      steps += minimumDistance;
+    }
+
+    if (deltaLat < deltaLong) {
+      steps += deltaLong - deltaLat;
+    } else if (deltaLong < deltaLat) {
+      steps += deltaLat - deltaLong;
+    }
+
+    return steps;
+  }
+
   double manhattanDistance(GpsCoordinates other) =>
     (latitude - other.latitude).abs() * metersPerLatitude +
     (longitude - other.longitude).abs() * metersPerLongitude;
@@ -58,6 +78,11 @@ extension GpsUtils on GpsCoordinates {
     longitude: longitude + other.longitude,
   );
 
+  GpsCoordinates operator -(GpsCoordinates other) => GpsCoordinates(
+    latitude: latitude - other.latitude,
+    longitude: longitude - other.longitude,
+  );
+
   String prettyPrint() => toProto3Json().toString();
 
   GpsCoordinates goForward(CardinalDirection orientation) => this + switch(orientation) {
diff --git a/lib/src/utils/receiver.dart b/lib/src/utils/receiver.dart
index d301496..c5837d8 100644
--- a/lib/src/utils/receiver.dart
+++ b/lib/src/utils/receiver.dart
@@ -1,21 +1,19 @@
 import "dart:async";
 
 mixin Receiver {
-  Completer<void>? _completer;
+  final Completer<bool> _completer = Completer<bool>();
 
   bool _hasValue = false;
 
   set hasValue(bool value) {
     _hasValue = value;
     if (!value) return;
-    _completer?.complete();
-    _completer = null;
+    if (!_completer.isCompleted) {
+      _completer.complete(true);
+    }
   }
 
   bool get hasValue => _hasValue;
 
-  Future<void> waitForValue() {
-    _completer = Completer<bool>();
-    return _completer!.future;
-  }
+  Future<void> waitForValue() => _completer.future;
 }
diff --git a/pubspec.lock b/pubspec.lock
index f67921e..34878c8 100644
--- a/pubspec.lock
+++ b/pubspec.lock
@@ -57,9 +57,11 @@ packages:
   burt_network:
     dependency: "direct main"
     description:
-      path: "../../burt_network"
-      relative: true
-    source: path
+      path: "."
+      ref: "2.3.0"
+      resolved-ref: "07cbb7b9add6e1a5221b696330998899644d5aed"
+      url: "https://github.com/BinghamtonRover/Networking.git"
+    source: git
     version: "2.3.0"
   collection:
     dependency: transitive
diff --git a/test/network_test.dart b/test/network_test.dart
index f6595f5..8c64bb6 100644
--- a/test/network_test.dart
+++ b/test/network_test.dart
@@ -1,6 +1,8 @@
+import "dart:async";
 import "dart:io";
 
 import "package:autonomy/autonomy.dart";
+import "package:autonomy/src/drive/drive_config.dart";
 import "package:burt_network/burt_network.dart";
 import "package:test/test.dart";
 
@@ -33,31 +35,40 @@ class MockSubsystems extends Service {
     if (!enabled) return;
     if (command.setLeft) left = command.left;
     if (command.setRight) right = command.right;
-    if (command.setThrottle) throttle = command.throttle;
-    if (throttle > 0) throttleFlag = true;
+    if (command.setThrottle) {
+      throttle = command.throttle;
+      throttleFlag = throttle > 0;
+    }
   }
 
   @override
   Future<void> dispose() async {
-    left = 0; right = 0;
-    throttle = 0; throttleFlag = false;
+    left = 0;
+    right = 0;
+    throttle = 0;
+    throttleFlag = false;
+    enabled = false;
     await socket.dispose();
   }
 }
 
 void main() => group("[Network]", tags: ["network"], () {
-  final subsystems = MockSubsystems();
+  var subsystems = MockSubsystems();
   final rover = RoverAutonomy();
-  rover.drive = SensorlessDrive(collection: rover, useGps: false, useImu: false);
+  rover.drive = RoverDrive(collection: rover, useGps: false, useImu: false);
 
-  Future<void> setup() async {
+  setUp(() async {
     Logger.level = LogLevel.off;
+    await subsystems.dispose();
+    subsystems = MockSubsystems();
     await subsystems.init();
     await rover.init();
-  }
+  });
 
-  setUp(setup);
-  tearDown(() async { await subsystems.dispose(); await rover.dispose(); });
+  tearDown(() async {
+    await subsystems.dispose();
+    await rover.dispose();
+  });
 
   test("Rover waits for all data to arrive", () async {
     final gps = GpsCoordinates(latitude: 1, longitude: 2);
@@ -92,18 +103,21 @@ void main() => group("[Network]", tags: ["network"], () {
     expect(rover.imu.hasValue, isTrue);
     expect(rover.video.hasValue, isTrue);
     expect(rover.hasValue, isTrue);
+
+    await Future<void>.delayed(const Duration(seconds: 1));
   });
 
-  test("Rover can drive", () async {
+  test("Rover can drive", retry: 5, () async {
     subsystems.enabled = true;
-    ServerUtils.subsystemsDestination = SocketInfo(
-      address: InternetAddress.loopbackIPv4,
-      port: 8001,
-    );
     final simulator = AutonomySimulator();
     simulator.gps = GpsSimulator(collection: simulator);
     simulator.imu = ImuSimulator(collection: simulator);
-    simulator.drive = SensorlessDrive(collection: simulator, useGps: false, useImu: false);
+    simulator.drive = RoverDrive(
+      collection: simulator,
+      useGps: false,
+      useImu: false,
+      config: tankConfig,
+    );
     await simulator.init();
 
     final origin = GpsCoordinates(latitude: 0, longitude: 0);
@@ -114,11 +128,18 @@ void main() => group("[Network]", tags: ["network"], () {
     expect(simulator.gps.isNear(origin), isTrue);
     expect(simulator.gps.isNear(oneMeter), isFalse);
 
-    const driveDelay = Duration(milliseconds: 10);
     expect(subsystems.throttleFlag, isFalse);
-    await simulator.drive.goForward();
-    await Future<void>.delayed(driveDelay);
+    final forwardFuture = simulator.drive.driveForward(oneMeter);
+    await Future<void>.delayed(simulator.drive.config.oneMeterDelay * 0.5);
     expect(subsystems.throttleFlag, isTrue);
+    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);
+    await forwardFuture;
+    await Future<void>.delayed(simulator.drive.config.oneMeterDelay * 0.5);
+    expect(subsystems.throttleFlag, isFalse);
     expect(subsystems.throttle, 0);
     expect(subsystems.left, 0);
     expect(subsystems.right, 0);
@@ -126,6 +147,7 @@ void main() => group("[Network]", tags: ["network"], () {
     expect(simulator.gps.isNear(oneMeter), isTrue);
 
     subsystems.enabled = false;
+    await subsystems.dispose();
     await simulator.dispose();
   });
 });
diff --git a/test/orchestrator_test.dart b/test/orchestrator_test.dart
index 82084eb..f8419b5 100644
--- a/test/orchestrator_test.dart
+++ b/test/orchestrator_test.dart
@@ -1,9 +1,12 @@
+import "dart:async";
+
+import "package:autonomy/src/drive/drive_config.dart";
 import "package:test/test.dart";
 import "package:autonomy/autonomy.dart";
 import "package:burt_network/burt_network.dart";
 
 void main() => group("[Orchestrator]", tags: ["orchestrator"], () {
-  setUp(() => Logger.level = LogLevel.off);
+  setUp(() => Logger.level = LogLevel.info);
   tearDown(() => Logger.level = LogLevel.off);
 
   test("Fails for invalid destinations", () async {
@@ -94,6 +97,7 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () {
     simulator.orchestrator = RoverOrchestrator(collection: simulator);
     simulator.pathfinder = RoverPathfinder(collection: simulator);
     simulator.gps = RoverGps(collection: simulator);
+    simulator.drive = RoverDrive(collection: simulator, useGps: true, useImu: false, config: tankConfig);
     await simulator.init();
 
     expect(simulator.gps.hasValue, isFalse);
@@ -103,13 +107,15 @@ void main() => group("[Orchestrator]", tags: ["orchestrator"], () {
     expect(GpsInterface.currentLatitude, 0);
     expect(simulator.orchestrator.statusMessage.state, AutonomyState.NO_SOLUTION);
 
-    simulator.gps.update(start);
+    simulator.gps.forceUpdate(start);
     await simulator.init();
     await simulator.waitForValue();
-    await simulator.orchestrator.onCommand(command);
     expect(simulator.hasValue, isTrue);
+    unawaited(simulator.orchestrator.onCommand(command));
+    await Future<void>.delayed(Duration.zero);
+    expect(simulator.orchestrator.currentCommand, isNotNull);
     expect(GpsInterface.currentLatitude, start.latitude);
-    expect(simulator.orchestrator.currentState, AutonomyState.AT_DESTINATION);
+    expect(simulator.orchestrator.currentState, AutonomyState.DRIVING);
 
     GpsInterface.currentLatitude = 0;
     await simulator.dispose();
diff --git a/test/path_test.dart b/test/path_test.dart
index f246b74..b877658 100644
--- a/test/path_test.dart
+++ b/test/path_test.dart
@@ -5,6 +5,14 @@ import "package:autonomy/interfaces.dart";
 import "package:autonomy/rover.dart";
 import "package:autonomy/simulator.dart";
 
+extension DriveFollowPath on DriveInterface {
+  Future<void> followPath(List<AutonomyAStarState> path) async {
+    for (final step in path) {
+      await driveState(step);
+    }
+  }
+}
+
 void main() => group("[Pathfinding]", tags: ["path"], () {
   setUp(() => Logger.level = LogLevel.off);
   tearDown(() => Logger.level = LogLevel.off);
@@ -33,7 +41,7 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
 
     var turnCount = 0;
     for (final step in path) {
-      if (step.direction.isTurn) {
+      if (step.instruction.isTurn) {
         turnCount++;
       }
       simulator.logger.trace(step.toString());
@@ -77,7 +85,7 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
     expect(path, isNotEmpty);
     for (final step in path) {
       simulator.logger.trace(step.toString());
-      expect(simulator.pathfinder.isObstacle(step.endPosition), isFalse);
+      expect(simulator.pathfinder.isObstacle(step.position), isFalse);
     }
     expect(path.length, 10, reason: "1 turn + 1 forward + 1 turn + 4 forward + 1 45 degree turn + 1 forward + 1 stop = 10 steps total");
     await simulator.drive.followPath(path);
@@ -87,7 +95,9 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
 
   test("Stress test", () async {
     final oldError = GpsUtils.maxErrorMeters;
+    final oldMoveLength = GpsUtils.moveLengthMeters;
     GpsUtils.maxErrorMeters = 1;
+    // GpsUtils.moveLengthMeters = 5;
     final simulator = AutonomySimulator();
     simulator.pathfinder = RoverPathfinder(collection: simulator);
     simulator.logger.trace("Starting from ${simulator.gps.coordinates.prettyPrint()}");
@@ -98,6 +108,7 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
     expect(path, isNotNull);
     await simulator.dispose();
     GpsUtils.maxErrorMeters = oldError;
+    GpsUtils.moveLengthMeters = oldMoveLength;
   });
 
   test("Impossible paths are reported", () async {
diff --git a/test/rover_test.dart b/test/rover_test.dart
index 9621d32..2563f64 100644
--- a/test/rover_test.dart
+++ b/test/rover_test.dart
@@ -37,13 +37,13 @@ void main() => group("[Rover]", tags: ["rover"], () {
 
     expect(rover.hasValue, isFalse);
     expect(rover.gps.hasValue, isFalse);
-    rover.gps.update(position);
+    rover.gps.forceUpdate(position);
     expect(rover.gps.hasValue, isTrue);
     expect(rover.hasValue, isFalse);
 
     expect(rover.hasValue, isFalse);
     expect(rover.imu.hasValue, isFalse);
-    rover.imu.update(orientation);
+    rover.imu.forceUpdate(orientation);
     expect(rover.imu.hasValue, isTrue);
     expect(rover.hasValue, isFalse);
 
@@ -68,11 +68,11 @@ Future<void> testPath(AutonomyInterface simulator) async {
   for (final transition in result) {
     simulator.logger.trace("  From: ${simulator.gps.coordinates.prettyPrint()} facing ${simulator.imu.heading}");
     simulator.logger.debug("  $transition");
-    await simulator.drive.goDirection(transition.direction);
-    expect(simulator.gps.isNear(transition.endPosition), isTrue);
+    await simulator.drive.driveState(transition);
+    expect(simulator.gps.isNear(transition.position), isTrue);
     simulator.logger.trace("New orientation: ${simulator.imu.heading}");
-    simulator.logger.trace("Expected orientation: ${transition.endOrientation}");
-    expect(simulator.imu.orientation, transition.endOrientation);
+    simulator.logger.trace("Expected orientation: ${transition.orientation}");
+    expect(simulator.imu.nearest, transition.orientation);
   }
 }
 
@@ -91,10 +91,10 @@ Future<void> testPath2(AutonomyInterface simulator) async {
   for (final transition in result) {
     simulator.logger.debug(transition.toString());
     simulator.logger.trace("  From: ${simulator.gps.coordinates.prettyPrint()}");
-    await simulator.drive.goDirection(transition.direction);
-    expect(simulator.gps.isNear(transition.endPosition), isTrue);
+    await simulator.drive.driveState(transition);
+    expect(simulator.gps.isNear(transition.position), isTrue);
     expect(simulator.pathfinder.isObstacle(simulator.gps.coordinates), isFalse);
-    expect(simulator.imu.orientation, transition.endOrientation);
+    expect(simulator.imu.nearest, transition.orientation);
     simulator.logger.trace("  To: ${simulator.gps.coordinates.prettyPrint()}");
   }
 }
diff --git a/test/sensor_test.dart b/test/sensor_test.dart
index dce2233..69557a1 100644
--- a/test/sensor_test.dart
+++ b/test/sensor_test.dart
@@ -1,11 +1,9 @@
+import "package:autonomy/autonomy.dart";
 import "package:test/test.dart";
 
 import "package:burt_network/protobuf.dart";
 import "package:burt_network/logging.dart";
 
-import "package:autonomy/interfaces.dart";
-import "package:autonomy/simulator.dart";
-
 const imuError = 2.5;
 const gpsPrecision = 7;
 
@@ -80,8 +78,8 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     final simulator = AutonomySimulator();
     final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError);
     final realImu = RoverImu(collection: simulator);
-    final north = OrientationUtils.north;
-    simulatedImu.update(north);
+    final north = CardinalDirection.north;
+    simulatedImu.update(north.orientation);
 
     var count = 0;
     for (var i = 0; i < 1000; i++) {
@@ -89,10 +87,10 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
       realImu.update(newData);
       simulator.logger.trace("Got new value: ${newData.heading}");
       simulator.logger.trace("  New heading: ${realImu.heading}");
-      simulator.logger.trace("  Real position: ${north.heading}");
+      simulator.logger.trace("  Real position: ${north.angle}");
       if (i < 10) continue;
-      simulator.logger.trace("  Values are similar: ${realImu.isNear(north.heading)}");
-      if (realImu.isNear(north.heading)) count++;
+      simulator.logger.trace("  Values are similar: ${realImu.isNear(CardinalDirection.north)}");
+      if (realImu.isNear(north)) count++;
     }
 
     final percentage = count / 1000;
@@ -100,12 +98,14 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage");
 
 
-    realImu.update(OrientationUtils.south);
-    expect(realImu.isNear(OrientationUtils.north.heading), isTrue);
+    realImu.forceUpdate(OrientationUtils.south);
+    expect(realImu.isNear(north), isTrue);
     await simulator.dispose();
   });
 
-  test("GPS noise when moving", () async {
+  test("GPS noise when moving",
+      skip: "GPS noise is reduced enough with RTK where filtering is not necessary",
+      () async {
     // Set up a simulated and real GPS, both starting at (0, 0)
     final oldError = GpsUtils.maxErrorMeters;
     GpsUtils.maxErrorMeters = 3;
@@ -114,7 +114,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     final simulatedGps = GpsSimulator(collection: simulator, maxError: GpsInterface.gpsError);
     var realCoordinates = GpsCoordinates();
     simulatedGps.update(realCoordinates);
-    realGps.update(realCoordinates);
+    realGps.forceUpdate(realCoordinates);
     expect(realGps.coordinates.isNear(realCoordinates), isTrue);
 
     // For each step forward, use the noisy GPS to update the real GPS.
@@ -122,7 +122,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     for (var step = 0; step < 1000; step++) {
       realCoordinates += GpsUtils.north;
       simulatedGps.update(realCoordinates);
-      realGps.update(simulatedGps.coordinates);
+      realGps.forceUpdate(simulatedGps.coordinates);
       simulator.logger.trace("New coordinate: ${realGps.coordinates.latitude.toStringAsFixed(5)} vs real position: ${realCoordinates.latitude.toStringAsFixed(5)}");
       simulator.logger.trace("  Difference: ${(realGps.latitude - realCoordinates.latitude).abs().toStringAsFixed(5)} < ${GpsUtils.epsilonLatitude.toStringAsFixed(5)}");
       if (step < 10) continue;
@@ -140,12 +140,12 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     GpsUtils.maxErrorMeters = oldError;
   });
 
-  test("IMU noise when moving", () async {
+  test("IMU noise when moving", skip: "IMU is currently accurate enough to not need filtering", () async {
     Logger.level = LogLevel.off;
     final simulator = AutonomySimulator();
     final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError);
     final realImu = RoverImu(collection: simulator);
-    final orientation = OrientationUtils.north;
+    final orientation = CardinalDirection.north.orientation;
     simulatedImu.update(orientation);
 
     var count = 0;
@@ -153,13 +153,13 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
       orientation.z += 1;
       simulatedImu.update(orientation);
       final newData = simulatedImu.raw;
-      realImu.update(newData);
+      realImu.forceUpdate(newData);
       simulator.logger.trace("Got new value: ${newData.heading}");
       simulator.logger.trace("  New heading: ${realImu.heading}");
       simulator.logger.trace("  Real position: ${orientation.heading}");
       if (i < 10) continue;
-      simulator.logger.trace("  Values are similar: ${realImu.isNear(orientation.heading)}");
-      if (realImu.isNear(orientation.heading)) count++;
+      simulator.logger.trace("  Values are similar: ${realImu.isNear(CardinalDirection.north)}");
+      if (realImu.isNear(CardinalDirection.north)) count++;
     }
 
     final percentage = count / 350;
@@ -168,9 +168,9 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     final badData = Orientation(z: 10);
     simulator.logger.info("Final orientation: ${realImu.heading}");
     simulator.logger.info("Bad orientation: ${badData.heading}");
-    realImu.update(badData);
+    realImu.forceUpdate(badData);
     simulator.logger.info("Unaffected orientation: ${realImu.heading}");
-    expect(realImu.isNear(orientation.heading), isTrue);
+    expect(realImu.isNear(CardinalDirection.north), isTrue);
     await simulator.dispose();
   });
 
@@ -179,7 +179,7 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     const utahLatitude = 38.406683;
     final utah = GpsCoordinates(latitude: utahLatitude);
 
-    simulator.gps.update(utah);
+    simulator.gps.forceUpdate(utah);
     expect(simulator.hasValue, isFalse);
     expect(GpsInterface.currentLatitude, 0);
 
@@ -203,42 +203,42 @@ void main() => group("[Sensors]", tags: ["sensors"], () {
     expect(simulator.gps.isNear(newYork), isFalse);
     expect(ocean.isNear(newYork), isFalse);
 
-    simulator.gps.update(newYork);
+    simulator.gps.forceUpdate(newYork);
     expect(simulator.gps.isNear(ocean), isFalse);
     expect(simulator.gps.isNear(newYork), isTrue);
 
     await simulator.dispose();
   });
 
-  test("IMU can handle values on the edge", () async {
-    Logger.level = LogLevel.off;
-    final simulator = AutonomySimulator();
-    final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError);
-    final realImu = RoverImu(collection: simulator);
-    final orientation = Orientation(z: 360);
-    simulatedImu.update(orientation);
-
-    var count = 0;
-    for (var i = 0; i < 1000; i++) {
-      final newData = simulatedImu.raw;
-      realImu.update(newData);
-      simulator.logger.trace("Got new value: ${newData.heading}");
-      simulator.logger.trace("  New heading: ${realImu.heading}");
-      simulator.logger.trace("  Real position: ${orientation.heading}");
-      if (i < 10) continue;
-      simulator.logger.trace("  Values are similar: ${realImu.isNear(orientation.heading)}");
-      if (realImu.isNear(orientation.heading)) count++;
-    }
-
-    final percentage = count / 1000;
-    expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage");
-
-    final badData = Orientation(z: 10);
-    simulator.logger.info("Final orientation: ${realImu.heading}");
-    simulator.logger.info("Bad orientation: ${badData.heading}");
-    realImu.update(badData);
-    simulator.logger.info("Unaffected orientation: ${realImu.heading}");
-    expect(realImu.isNear(orientation.heading), isTrue);
-    await simulator.dispose();
-  });
+  // test("IMU can handle values on the edge", () async {
+  //   Logger.level = LogLevel.off;
+  //   final simulator = AutonomySimulator();
+  //   final simulatedImu = ImuSimulator(collection: simulator, maxError: imuError);
+  //   final realImu = RoverImu(collection: simulator);
+  //   final orientation = Orientation(z: 360);
+  //   simulatedImu.update(orientation);
+
+  //   var count = 0;
+  //   for (var i = 0; i < 1000; i++) {
+  //     final newData = simulatedImu.raw;
+  //     realImu.update(newData);
+  //     simulator.logger.trace("Got new value: ${newData.heading}");
+  //     simulator.logger.trace("  New heading: ${realImu.heading}");
+  //     simulator.logger.trace("  Real position: ${orientation.heading}");
+  //     if (i < 10) continue;
+  //     simulator.logger.trace("  Values are similar: ${realImu.isNear(orientation.heading)}");
+  //     if (realImu.isNear(orientation.heading)) count++;
+  //   }
+
+  //   final percentage = count / 1000;
+  //   expect(percentage, greaterThan(0.75), reason: "IMU should be accurate >75% of the time: $percentage");
+
+  //   final badData = Orientation(z: 10);
+  //   simulator.logger.info("Final orientation: ${realImu.heading}");
+  //   simulator.logger.info("Bad orientation: ${badData.heading}");
+  //   realImu.update(badData);
+  //   simulator.logger.info("Unaffected orientation: ${realImu.heading}");
+  //   expect(realImu.isNear(orientation.heading), isTrue);
+  //   await simulator.dispose();
+  // });
 });

From 1ceb9bde4c485d56fee3c21ecf3ad5bd17ad45f5 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Mon, 16 Dec 2024 19:28:42 -0500
Subject: [PATCH 15/21] Account for move length in heuristic

---
 lib/src/utils/gps_utils.dart | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/lib/src/utils/gps_utils.dart b/lib/src/utils/gps_utils.dart
index 983dcc5..9a5bb6e 100644
--- a/lib/src/utils/gps_utils.dart
+++ b/lib/src/utils/gps_utils.dart
@@ -42,13 +42,13 @@ extension GpsUtils on GpsCoordinates {
 
     final minimumDistance = min(deltaLat, deltaLong);
     if (minimumDistance >= moveLengthMeters) {
-      steps += minimumDistance;
+      steps += minimumDistance / moveLengthMeters;
     }
 
     if (deltaLat < deltaLong) {
-      steps += deltaLong - deltaLat;
+      steps += (deltaLong - deltaLat) / moveLengthMeters;
     } else if (deltaLong < deltaLat) {
-      steps += deltaLat - deltaLong;
+      steps += (deltaLat - deltaLong) / moveLengthMeters;
     }
 
     return steps;

From d0301001b5fd1c5b4618332641bcd343ada7708a Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Mon, 16 Dec 2024 19:45:52 -0500
Subject: [PATCH 16/21] Simplified heuristic calculation (again)

---
 lib/src/utils/gps_utils.dart | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/lib/src/utils/gps_utils.dart b/lib/src/utils/gps_utils.dart
index 9a5bb6e..92736bc 100644
--- a/lib/src/utils/gps_utils.dart
+++ b/lib/src/utils/gps_utils.dart
@@ -45,10 +45,10 @@ extension GpsUtils on GpsCoordinates {
       steps += minimumDistance / moveLengthMeters;
     }
 
-    if (deltaLat < deltaLong) {
-      steps += (deltaLong - deltaLat) / moveLengthMeters;
-    } else if (deltaLong < deltaLat) {
-      steps += (deltaLat - deltaLong) / moveLengthMeters;
+    final translationDelta = (deltaLat - deltaLong).abs();
+
+    if (translationDelta >= moveLengthMeters) {
+      steps += translationDelta / moveLengthMeters;
     }
 
     return steps;

From 4a03f60378a318caf0e61da34537a411fc5f5d38 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Wed, 18 Dec 2024 18:19:42 -0500
Subject: [PATCH 17/21] Fix path following not stopping after restart

---
 lib/src/orchestrator/rover_orchestrator.dart | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/lib/src/orchestrator/rover_orchestrator.dart b/lib/src/orchestrator/rover_orchestrator.dart
index 7d536c8..aecf35f 100644
--- a/lib/src/orchestrator/rover_orchestrator.dart
+++ b/lib/src/orchestrator/rover_orchestrator.dart
@@ -66,6 +66,10 @@ class RoverOrchestrator extends OrchestratorInterface with ValueReporter {
       for (final state in path) {
         collection.logger.debug(state.toString());
         await collection.drive.driveState(state);
+        if (currentCommand == null || currentPath == null) {
+          collection.logger.debug("Aborting path, command was canceled");
+          return;
+        }
         traversed.add(state.position);
         // if (state.direction != DriveDirection.forward) continue;
         if (count++ == 5) break;

From 9657bb4b28305d2ad794fc3c61be1b8b2b627c74 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Wed, 18 Dec 2024 18:20:23 -0500
Subject: [PATCH 18/21] Fixed heuristic math to use only distance

---
 lib/src/utils/gps_utils.dart | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/lib/src/utils/gps_utils.dart b/lib/src/utils/gps_utils.dart
index 92736bc..27710a9 100644
--- a/lib/src/utils/gps_utils.dart
+++ b/lib/src/utils/gps_utils.dart
@@ -35,23 +35,23 @@ extension GpsUtils on GpsCoordinates {
   );
 
   double heuristicDistance(GpsCoordinates other) {
-    var steps = 0.0;
+    var distance = 0.0;
     final delta = (this - other).inMeters;
     final deltaLat = delta.lat.abs();
     final deltaLong = delta.long.abs();
 
     final minimumDistance = min(deltaLat, deltaLong);
     if (minimumDistance >= moveLengthMeters) {
-      steps += minimumDistance / moveLengthMeters;
+      distance += (minimumDistance / moveLengthMeters) * sqrt2;
     }
 
     final translationDelta = (deltaLat - deltaLong).abs();
 
     if (translationDelta >= moveLengthMeters) {
-      steps += translationDelta / moveLengthMeters;
+      distance += translationDelta / moveLengthMeters;
     }
 
-    return steps;
+    return distance;
   }
 
   double manhattanDistance(GpsCoordinates other) =>

From 4b50d0af5d17a149b83ab2eb3277bee657264a0c Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Wed, 18 Dec 2024 18:20:33 -0500
Subject: [PATCH 19/21] Add more path tests

---
 test/path_test.dart | 49 ++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 48 insertions(+), 1 deletion(-)

diff --git a/test/path_test.dart b/test/path_test.dart
index b877658..246703c 100644
--- a/test/path_test.dart
+++ b/test/path_test.dart
@@ -97,7 +97,7 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
     final oldError = GpsUtils.maxErrorMeters;
     final oldMoveLength = GpsUtils.moveLengthMeters;
     GpsUtils.maxErrorMeters = 1;
-    // GpsUtils.moveLengthMeters = 5;
+    GpsUtils.moveLengthMeters = 1;
     final simulator = AutonomySimulator();
     simulator.pathfinder = RoverPathfinder(collection: simulator);
     simulator.logger.trace("Starting from ${simulator.gps.coordinates.prettyPrint()}");
@@ -127,4 +127,51 @@ void main() => group("[Pathfinding]", tags: ["path"], () {
     expect(path, isNull);
     await simulator.dispose();
   });
+
+  group("diagonal turns", () {
+    test("path chooses to move diagonally", () async {
+      final simulator = AutonomySimulator();
+      simulator.pathfinder = RoverPathfinder(collection: simulator);
+      final destination = (lat: 5, long: 5).toGps();
+      final path = simulator.pathfinder.getPath(destination);
+      expect(path, isNotNull);
+      expect(path!.where((state) => state.instruction == DriveDirection.forward).length, 5);
+      expect(path[1].instruction, DriveDirection.quarterRight);
+      await simulator.dispose();
+    });
+
+    test("doesn't drive through an obstacle", () async {
+      final simulator = AutonomySimulator();
+      simulator.pathfinder = RoverPathfinder(collection: simulator);
+      final destination = (lat: 5, long: 5).toGps();
+      final obstacles = {
+        (lat: 1, long: 0).toGps(), /* Destination */
+              /* Rover */           (lat: 0, long: 1).toGps(),
+      };
+      for (final obstacle in obstacles) {
+        simulator.pathfinder.recordObstacle(obstacle);
+      }
+      final path = simulator.pathfinder.getPath(destination);
+      expect(path, isNotNull);
+      expect(path!.where((state) => state.instruction == DriveDirection.forward).length, greaterThan(2));
+      await simulator.dispose();
+    });
+
+    test("doesn't drive through an obstacle", () async {
+      final simulator = AutonomySimulator();
+      simulator.pathfinder = RoverPathfinder(collection: simulator);
+      final destination = (lat: 5, long: 5).toGps();
+      final obstacles = {
+        (lat: 1, long: 0).toGps(), /* Destination */
+              /* Rover */
+      };
+      for (final obstacle in obstacles) {
+        simulator.pathfinder.recordObstacle(obstacle);
+      }
+      final path = simulator.pathfinder.getPath(destination);
+      expect(path, isNotNull);
+      expect(path!.where((state) => state.instruction == DriveDirection.forward).length, greaterThan(1));
+      await simulator.dispose();
+    });
+  });
 });

From 3bfe8cb266b35608e0f570949e9c3a0e40fb31cc Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Fri, 20 Dec 2024 01:02:45 -0500
Subject: [PATCH 20/21] Fixed excessive turns (no idea why this fixes it but
 whatever)

---
 lib/src/utils/gps_utils.dart | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/src/utils/gps_utils.dart b/lib/src/utils/gps_utils.dart
index 27710a9..8be55e2 100644
--- a/lib/src/utils/gps_utils.dart
+++ b/lib/src/utils/gps_utils.dart
@@ -42,7 +42,7 @@ extension GpsUtils on GpsCoordinates {
 
     final minimumDistance = min(deltaLat, deltaLong);
     if (minimumDistance >= moveLengthMeters) {
-      distance += (minimumDistance / moveLengthMeters) * sqrt2;
+      distance += (minimumDistance ~/ moveLengthMeters) * sqrt2;
     }
 
     final translationDelta = (deltaLat - deltaLong).abs();

From ede67745d48bc1a0ab925eb6ec12624759c0fc80 Mon Sep 17 00:00:00 2001
From: Gold87 <91761103+Gold872@users.noreply.github.com>
Date: Mon, 23 Dec 2024 21:50:04 -0500
Subject: [PATCH 21/21] Fixed sensor drive

---
 lib/src/video/video_interface.dart | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/lib/src/video/video_interface.dart b/lib/src/video/video_interface.dart
index e5923c2..77ea2ad 100644
--- a/lib/src/video/video_interface.dart
+++ b/lib/src/video/video_interface.dart
@@ -11,6 +11,6 @@ abstract class VideoInterface extends Service with Receiver {
 
   void updateFrame(VideoData newData);
 
-  // double get arucoSize => data.arucoSize;
-  // double get arucoPosition => data.arucoPosition;
+  double get arucoSize => 0; // data.arucoSize;
+  double get arucoPosition => 0; // data.arucoPosition;
 }