URC ready (#10)

Co-authored-by: Binghamton University Rover Team <[email protected]>

bin/data.dart

@@ -33,12 +33,18 @@ Future<void> main() async {
   final current = SwingingIterator(0, 30, 0.1);
   final motor = SwingingIterator(0, pi, 0.01);
   final motor2 = SwingingIterator(0, 2*pi, 0.05);
+  final roll = SwingingIterator(-45, 45, 1);
+  final pitch = SwingingIterator(-45, 45, 1);
+  final yaw = SwingingIterator(-45, 45, 1);
   while (true) {
     throttle.moveNext();
     voltage.moveNext();
     current.moveNext();
     motor.moveNext();
     motor2.moveNext();
+    roll.moveNext();
+    pitch.moveNext();
+    yaw.moveNext();
     final data = DriveData(
       left: 1, 
       setLeft: true, 
@@ -48,16 +54,27 @@ Future<void> main() async {
       setThrottle: true, 
       batteryVoltage: voltage.current, 
       batteryCurrent: current.current,
+      version: Version(major: 1),
     ); 
     server.sendMessage(data);
     final data2 = ArmData(
       base: MotorData(angle: motor2.current), 
       shoulder: MotorData(angle: motor.current), 
       elbow: MotorData(angle: motor.current),
+      version: Version(major: 1),
     );
     server.sendMessage(data2);
-    final data3 = GripperData(lift: MotorData(angle: motor.current));
+    final data3 = GripperData(lift: MotorData(angle: motor.current), version: Version(major: 1));
     server.sendMessage(data3);
+    final data4 = RoverPosition(
+      orientation: Orientation(
+        x: roll.current,
+        y: pitch.current, 
+        z: yaw.current,
+      ),
+      version: Version(major: 1),
+    );
+    server.sendMessage(data4);
     await Future<void>.delayed(const Duration(milliseconds: 10));
   }
 }

bin/serial.dart

@@ -1,6 +1,7 @@
 import "dart:io";
 import "dart:typed_data";
 import "package:burt_network/burt_network.dart";
+import "package:collection/collection.dart";
 import "package:libserialport/libserialport.dart";
 
 final logger = BurtLogger();
@@ -37,12 +38,35 @@ Future<void> listenToFirmware(String port) async {
   device.stream?.listen(process);
 }
 
+typedef ProtoConstructor = Message Function(List<int> data);
+
+ProtoConstructor? getDataConstructor(Device device) => switch (device) {
+  Device.DRIVE => DriveData.fromBuffer,
+  Device.ARM => ArmData.fromBuffer,
+  Device.GRIPPER => GripperData.fromBuffer,
+  Device.SCIENCE => GripperData.fromBuffer,
+  _ => null,
+};
+
+ProtoConstructor? constructor;
+
 void main(List<String> args) async {
   if (args.isEmpty) {
     logger.info("Ports: ${SerialPort.availablePorts}");
     return;
   } else if (args.contains("-h") || args.contains("--help")) {
-    logger.info("Usage: dart run -r :serial [port] [-a | --ascii]");
+    logger.info("Usage: dart run -r :serial [-a | --ascii] [port device]");
+    return;
+  }
+  final deviceName = args.last;
+  final device = Device.values.firstWhereOrNull((d) => d.name.toLowerCase() == deviceName.toLowerCase());
+  if (device == null) {
+    logger.error("Enter a device name as the last argument. Unrecognized device: $deviceName");
+    return;
+  }
+  constructor = getDataConstructor(device);
+  if (constructor == null) {
+    logger.error("Unsupported serial device: $deviceName");
     return;
   }
   var port = args.first;
@@ -64,7 +88,7 @@ void process(Uint8List buffer) {
     logger.debug("Got string: $s");	
   } else {
     try {
-      final data = ScienceData.fromBuffer(buffer);
+      final data = constructor!(buffer);
       logger.debug("Got data: ${data.toProto3Json()}");
     } catch (error) {
       logger.error("Could not decode DriveData: $error\n  Buffer: $buffer");

lib/src/devices/gps.dart

@@ -81,6 +81,8 @@ class GpsReader extends Service {
         return false;
       }
       _subscription = device.stream.listen(_handlePacket);
+	device.startListening();
+	logger.info("Reading GPS over port $gpsPort");
       return true;
     } catch (error) {
       logger.critical("Could not open GPS", body: "Port $gpsPort, Error=$error");

lib/src/devices/imu.dart

@@ -36,7 +36,7 @@ class ImuReader extends Service {
       );
       if (orientation.x.isZero() || orientation.y.isZero() || orientation.z.isZero()) return;
       if (orientation.x.abs() > 360 || orientation.y.abs() > 360 || orientation.z.abs() > 360) return;
-      final position = RoverPosition(orientation: orientation);
+      final position = RoverPosition(orientation: orientation, version: Version(major: 1, minor: 0));
       collection.server.sendMessage(position);
     } catch (error) { /* Ignore corrupt data */ }
   }

lib/src/server.dart

@@ -12,6 +12,7 @@ class SubsystemsServer extends RoverServer {
 
 	@override
 	void onMessage(WrappedMessage wrapper) {
+    if (wrapper.name == RoverPosition().messageName) return;
     collection.sendWrapper(wrapper);
 	}
 

lib/subsystems.dart

@@ -75,7 +75,7 @@ class SubsystemsCollection extends MessageService {
   void sendWrapper(WrappedMessage wrapper) {
     if (!isReady) return;
     if (collection.serial.sendWrapper(wrapper)) return;
-    collection.can.sendWrapper(wrapper);
+//    collection.can.sendWrapper(wrapper);
   }
 
   /// Stops all the hardware from moving.