Removed ServerUtils and refactored Aruco drive code

lib/src/drive/drive_commands.dart

@@ -1,17 +1,15 @@
 import "package:autonomy/interfaces.dart";
 import "package:burt_network/protobuf.dart";
 
-mixin RoverDriveCommands {
-  AutonomyInterface get collection;
-
+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");
-    collection.server.sendCommand(DriveCommand(throttle: throttle, setThrottle: true));
+    sendCommand(DriveCommand(throttle: throttle, setThrottle: true));
   }
 
   /// Sets the speeds of the left and right wheels, using differential steering.
@@ -20,8 +18,8 @@ mixin RoverDriveCommands {
   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));
+    sendCommand(DriveCommand(left: left, setLeft: true));
+    sendCommand(DriveCommand(right: right, setRight: true));
   }
 
   void stopMotors() {
@@ -37,6 +35,6 @@ mixin RoverDriveCommands {
   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);
+    sendCommand(command);
   }
 }

lib/src/drive/drive_config.dart

@@ -1,27 +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",
 );

lib/src/drive/drive_interface.dart

@@ -26,6 +26,9 @@ abstract class DriveInterface extends Service {
 
   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].
@@ -45,7 +48,7 @@ abstract class DriveInterface extends Service {
 
   void setLedStrip(ProtoColor color, {bool blink = false}) {
     final command = DriveCommand(color: color, blink: blink ? BoolState.YES : BoolState.NO);
-    collection.server.sendCommand(command);
+    sendCommand(command);
   }
 
   Future<bool> spinForAruco() async => false;

lib/src/drive/sensor_drive.dart

@@ -28,7 +28,7 @@ class SensorDrive extends DriveInterface with RoverDriveCommands {
     collection.logger.info("Driving forward one meter");
     setThrottle(config.forwardThrottle);
     moveForward();
-    await waitFor(() => collection.gps.coordinates.isNear(state.position));
+    await waitFor(() => collection.gps.isNear(state.position));
     await stop();
   }
 
@@ -62,41 +62,26 @@ class SensorDrive extends DriveInterface with RoverDriveCommands {
 
   @override
   Future<bool> spinForAruco() async {
-    for (var i = 0; i < 16; i++) {
-      setThrottle(config.turnThrottle);
-      spinLeft();
-      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;
+    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();
-    // 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 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();
   }
 }

lib/utils.dart

@@ -6,6 +6,5 @@ export "src/utils/imu_utils.dart";
 export "src/utils/pid_controller.dart";
 export "src/utils/receiver.dart";
 export "src/utils/reporter.dart";
-export "src/utils/server.dart";
 
 export "package:burt_network/burt_network.dart" show Service;