Turning with IMU works

bin/task.dart

@@ -17,9 +17,13 @@ void main() async {
   simulator.detector = DetectorSimulator(collection: simulator, obstacles: obstacles);
   simulator.pathfinder = RoverPathfinder(collection: simulator);
   simulator.orchestrator = RoverOrchestrator(collection: simulator);
-  simulator.drive = SensorlessDrive(collection: simulator, useImu: false, useGps: false);
+  simulator.drive = RoverDrive(collection: simulator, useImu: true, useGps: false);
+  simulator.gps = GpsSimulator(collection: simulator);
+//  simulator.drive = DriveSimulator(collection: simulator);
   await simulator.init();
-  // await simulator.waitForValue();
-	await simulator.drive.faceNorth();
+  await simulator.imu.waitForValue();
+//	await simulator.drive.faceNorth();
   await simulator.server.waitForConnection();
+	print("Ready");
+
 }

bin/test.dart

@@ -8,10 +8,17 @@ void main() async {
   final rover = RoverAutonomy();
   rover.gps = RoverGps(collection: rover);
   rover.imu = RoverImu(collection: rover);
-  rover.drive = RoverDrive(collection: rover, useGps: false, useImu: false);
+  rover.drive = RoverDrive(collection: rover, useGps: false, useImu: true);
 
   await rover.init();
+  print("Waiting for readings");
+//  await rover.waitForReadings();
+//  await rover.waitForConnection();
 
-  // rover.logger.info("Done");
-  // await rover.dispose();
+  rover.logger.info("Starting");
+  await rover.drive.turnLeft();
+await rover.drive.turnRight();
+
+  rover.logger.info("Done");
+  await rover.dispose();
 }

lib/src/interfaces/autonomy.dart

@@ -44,8 +44,8 @@ abstract class AutonomyInterface extends Service with Receiver {
     await detector.dispose();
     await video.dispose();
     await orchestrator.dispose();
-    await server.dispose();
     logger.info("Autonomy disposed");
+    await server.dispose();
   }
 
   Future<void> restart() async {

lib/src/interfaces/gps_utils.dart

@@ -23,10 +23,11 @@ extension GpsUtils on GpsCoordinates {
     GpsCoordinates(latitude: -1 * latitudePerMeter);
 
   // Taken from https://stackoverflow.com/a/39540339/9392211
-  static const metersPerLatitude = 111.32 * 1000;  // 111.32 km
+  // 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;
+  static double get metersPerLongitude => 1;
+//    40075 * cos( GpsInterface.currentLatitude * radiansPerDegree ) / 360 * 1000;
 
   static double get latitudePerMeter => 1 / metersPerLatitude;
   static double get longitudePerMeter => 1 / metersPerLongitude;

lib/src/interfaces/orchestrator.dart

@@ -18,7 +18,7 @@ abstract class OrchestratorInterface extends Service {
       return;
     }
 
-    if (!collection.hasValue) {
+    if (!collection.hasValue && false) {
       collection.logger.error("Sensors haven't gotten any readings yet!");
       currentState = AutonomyState.NO_SOLUTION;
       return;

lib/src/rover/drive/sensor.dart

@@ -19,17 +19,8 @@ class SensorDrive extends DriveInterface with RoverMotors {
 
   Future<void> waitFor(bool Function() predicate) async {
     while (!predicate()) {
-//	collection.logger.debug("Next turning loop");
-//      collection.imu.hasValue = false;
-//      setThrottle(maxThrottle);
-//      setSpeeds(left: -1, right: 1);
       await Future<void>.delayed(predicateDelay);
-//	if (!collection.imu.hasValue) {
-//	collection.logger.trace("IMU has value: ${collection.imu.hasValue}");
-//	  await stop();
-//		collection.logger.warning("Checked for IMU value but didn't find it");
-//	}
-//      await collection.imu.waitForValue();
+      await collection.imu.waitForValue();
     }
   }
 
@@ -41,7 +32,7 @@ class SensorDrive extends DriveInterface with RoverMotors {
 
   @override
   Future<void> goForward() async {
-    final orientation = collection.imu.orientation;
+   final orientation = collection.imu.orientation;
     final currentCoordinates = collection.gps.coordinates;
     final destination = currentCoordinates.goForward(orientation!);
 
@@ -78,27 +69,32 @@ class SensorDrive extends DriveInterface with RoverMotors {
 
   @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(maxThrottle);
     setSpeeds(left: -1, right: 1);
     await waitFor(() => collection.imu.orientation == destination);
     await stop();
-	this.orientation = this.orientation.turnLeft();
-
+    this.orientation = this.orientation.turnLeft();
   }
 
   @override
   Future<void> turnRight() async {
-    // TODO: Allow corrective turns
+      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(maxThrottle);

lib/src/rover/drive/timed.dart

@@ -5,8 +5,8 @@ import "motors.dart";
 class TimedDrive extends DriveInterface with RoverMotors {
   static const maxThrottle = 0.1;
   static const turnThrottle = 0.1;
-  static const oneMeterDelay = Duration(seconds: 6);
-  static const turnDelay = Duration(milliseconds: 5500);
+  static const oneMeterDelay = Duration(milliseconds: 5500);
+  static const turnDelay = Duration(milliseconds: 4500);
 
   TimedDrive({required super.collection});
 

lib/src/rover/imu.dart

@@ -30,7 +30,7 @@ class RoverImu extends ImuInterface {
   void update(Orientation newValue) {
     //  _zCorrector.addValue(newValue.heading);
     //	collection.logger.trace("Got IMU value");
-    //	print("Got imu");
+    print("Got imu: ${newValue.heading}. Direction: ${collection.drive.orientation}");
     hasValue = true;
     value = newValue;
 	}

lib/src/rover/video.dart

@@ -25,8 +25,8 @@ class RoverVideo extends VideoInterface {
     if (data.arucoDetected == BoolState.YES) {
       flag = true;
       Timer(const Duration(seconds: 3), () => flag = false);
+      collection.logger.info("Is ArUco detected: ${data.arucoDetected}");
     }
-    collection.logger.info("Is ArUco detected: ${data.arucoDetected}");
     hasValue  = true;
   }
 }

lib/src/simulator/detector.dart

@@ -15,6 +15,7 @@ class DetectorSimulator extends DetectorInterface {
   static const slopedLatitude = -5;
 
   final List<SimulatedObstacle> obstacles;
+  final Set<GpsCoordinates> found = {};
 
   DetectorSimulator({required super.collection, required this.obstacles});
 
@@ -29,8 +30,9 @@ class DetectorSimulator extends DetectorInterface {
     final coordinates = collection.gps.coordinates;
     var result = false;
     for (final obstacle in obstacles) {
-      if (!obstacle.isNear(coordinates)) continue;
+      if (!obstacle.isNear(coordinates) || found.contains(obstacle.coordinates)) continue;
       result = true;
+      found.add(obstacle.coordinates);
       collection.pathfinder.recordObstacle(obstacle.coordinates);
     }
     return result;

pubspec.lock

@@ -57,7 +57,7 @@ packages:
   burt_network:
     dependency: "direct main"
     description:
-      path: "../../burt_network"
+      path: "../burt_network"
       relative: true
     source: path
     version: "2.0.0"
@@ -240,11 +240,9 @@ packages:
   opencv_ffi:
     dependency: "direct main"
     description:
-      path: "."
-      ref: HEAD
-      resolved-ref: fb721ce518faa62b470c73ae58edfe825a5f9052
-      url: "https://github.com/BinghamtonRover/OpenCV-FFI.git"
-    source: git
+      path: "../opencv_ffi"
+      relative: true
+    source: path
     version: "1.2.0"
   package_config:
     dependency: transitive