Add diagonal movement

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();

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));
 }

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,13 +38,43 @@ enum DriveOrientation {
     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,
   };
 }
 
@@ -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 {

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,
   };
 }

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();
 }

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++) {

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();
+  }
 }

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...");

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();
+  }
 }

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");
 }