Tank implementation using rpi_gpio

bin/tank.dart

@@ -0,0 +1,8 @@
+
+
+import "package:autonomy/tank.dart";
+
+void main() async {
+  final tank = Tank();
+  await tank.init();
+}

bin/task.dart

@@ -17,13 +17,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, useGps: false);
   simulator.gps = GpsSimulator(collection: simulator);
-//  simulator.drive = DriveSimulator(collection: simulator);
   await simulator.init();
   await simulator.imu.waitForValue();
-//	await simulator.drive.faceNorth();
   await simulator.server.waitForConnection();
-	print("Ready");
 
 }

bin/test.dart

@@ -8,17 +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: true);
-  
+  rover.drive = RoverDrive(collection: rover, useGps: false);
+
   await rover.init();
-  print("Waiting for readings");
-//  await rover.waitForReadings();
-//  await rover.waitForConnection();
+  rover.logger.info("Waiting for readings");
+  await rover.waitForValue();
+  await rover.server.waitForConnection();
 
   rover.logger.info("Starting");
   await rover.drive.turnLeft();
-await rover.drive.turnRight();
- 
+  await rover.drive.turnRight();
+
   rover.logger.info("Done");
   await rover.dispose();
 }

lib/src/interfaces/autonomy.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) {

lib/src/interfaces/orchestrator.dart

@@ -18,7 +18,7 @@ abstract class OrchestratorInterface extends Service {
       return;
     }
 
-    if (!collection.hasValue && false) {
+    if (!collection.hasValue) {
       collection.logger.error("Sensors haven't gotten any readings yet!");
       currentState = AutonomyState.NO_SOLUTION;
       return;
@@ -35,7 +35,6 @@ abstract class OrchestratorInterface extends Service {
 
   @override
   Future<bool> init() async {
-    print("Orchestrator init 2");
     collection.server.messages.onMessage(
       name: AutonomyCommand().messageName,
       constructor: AutonomyCommand.fromBuffer,

lib/src/interfaces/reporter.dart

@@ -13,7 +13,7 @@ mixin ValueReporter on Service {
   @override
   Future<bool> init() async {
     timer = Timer.periodic(reportInterval, (timer) => _reportValue());
-    return await super.init();
+    return super.init();
   }
 
   @override

lib/src/rover/drive/sensor.dart

@@ -79,7 +79,6 @@ 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);

lib/src/rover/imu.dart

@@ -30,7 +30,6 @@ class RoverImu extends ImuInterface {
   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;
 	}

lib/tank.dart

@@ -0,0 +1,107 @@
+import "dart:async";
+
+import "package:burt_network/burt_network.dart";
+import "package:rpi_gpio/gpio.dart";
+import "package:rpi_gpio/rpi_gpio.dart";
+
+const leftPinNumber = 13;
+const rightPinNumber = 14;
+
+const minVelocity = -1.0;
+const maxVelocity = 1.0;
+
+const pwmMaxPos = 82500.0;
+const pwmMinPos = 76500.0;
+const pwmZero = 75000.0;
+const pwmMinNeg = 73500.0;
+const pwmMaxNeg = 67500.0;
+
+const dataInterval = Duration(milliseconds: 250);
+final driveVersion = Version(major: 1, minor: 1);
+
+class Tank extends Service {
+  late final server = RoverSocket(port: 8001, device: Device.SUBSYSTEMS, collection: this);
+  late final logger = BurtLogger(socket: server);
+
+  double _leftSpeed = 0;
+  double _rightSpeed = 0;
+  double _throttle = 0;
+
+  RpiGpio? gpio;
+  GpioPwm? leftPin;
+  GpioPwm? rightPin;
+
+  StreamSubscription<DriveCommand>? _subscription;
+  Timer? _dataTimer;
+
+  @override
+  Future<bool> init() async {
+    try {
+      await server.init();
+      _subscription = server.messages.onMessage<DriveCommand>(
+        name: DriveCommand().messageName,
+        constructor: DriveCommand.fromBuffer,
+        callback: _handleDriveCommand,
+      );
+      _dataTimer = Timer.periodic(dataInterval, _sendData);
+      gpio = await initialize_RpiGpio();
+      leftPin = gpio?.pwm(leftPinNumber);
+      rightPin = gpio?.pwm(rightPinNumber);
+      return true;
+    } catch (error) {
+      logger.critical("Could not initialize the Tank", body: error.toString());
+      return false;
+    }
+  }
+
+  @override
+  Future<void> dispose() async {
+    _dataTimer?.cancel();
+    await _subscription?.cancel();
+    await server.dispose();
+    await gpio?.dispose();
+  }
+
+  @override
+  Future<void> onDisconnect() async {
+    _leftSpeed = 0;
+    _rightSpeed = 0;
+    _throttle = 0;
+    _updateMotors();
+    await super.onDisconnect();
+  }
+
+  void _sendData(Timer t) => server.sendMessage(DriveData(
+    setLeft: true, left: _leftSpeed,
+    setRight: true, right: _rightSpeed,
+    setThrottle: true, throttle: _throttle,
+    version: driveVersion,
+  ),);
+
+  void _handleDriveCommand(DriveCommand command) {
+    _leftSpeed = command.setLeft ? command.left : _leftSpeed;
+    _rightSpeed = command.setRight ? command.right : _rightSpeed;
+    _throttle = command.setThrottle ? command.throttle : _throttle;
+    _updateMotors();
+  }
+
+  void _updateMotors() {
+    if (gpio == null) return;
+    _setVelocity(leftPin!, _leftSpeed * _throttle);
+    _setVelocity(rightPin!, _rightSpeed * _throttle);
+  }
+
+  void _setVelocity(GpioPwm pin, double speed) {
+    var pwm = 0.0;
+    var velocity = -speed;  // not sure why we negate here
+    velocity = velocity.clamp(minVelocity, maxVelocity);
+    if (velocity == 0) {
+      pwm = pwmZero;
+    } else if (velocity > 0) {
+      pwm = pwmMinPos + velocity * (pwmMaxPos - pwmMinPos) / maxVelocity;
+    } else if (velocity < 0) {
+      pwm = pwmMinNeg + velocity * (pwmMaxNeg - pwmMinNeg) / minVelocity;
+    }
+    pin.dutyCycle = pwm.toInt();
+  }
+}

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,9 +240,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
@@ -292,6 +294,14 @@ packages:
       url: "https://pub.dev"
     source: hosted
     version: "2.1.4"
+  rpi_gpio:
+    dependency: "direct main"
+    description:
+      name: rpi_gpio
+      sha256: "7825b7ff7f5380ac966163368001a17befcaf8780822ae952d9783272c3913e5"
+      url: "https://pub.dev"
+    source: hosted
+    version: "0.10.0"
   shelf:
     dependency: transitive
     description:

pubspec.yaml

@@ -14,6 +14,7 @@ dependencies:
     git: https://github.com/BinghamtonRover/Dart-Networking.git
   a_star: ^3.0.0
   meta: ^1.11.0
+  rpi_gpio: ^0.10.0
 
 dev_dependencies:
   test: ^1.21.0