From bb5cc9693ba348d0e8517d28eda057d94ad54741 Mon Sep 17 00:00:00 2001 From: Levi Lesches Date: Tue, 12 Nov 2024 23:17:46 -0500 Subject: [PATCH 1/4] Untested tank code --- lib/src/tank/collection.dart | 36 ++++++++++++++++++++++ lib/src/tank/drive.dart | 58 ++++++++++++++++++++++++++++++++++++ lib/src/tank/gpio.dart | 55 ++++++++++++++++++++++++++++++++++ pubspec.lock | 18 ++++++++--- pubspec.yaml | 1 + 5 files changed, 164 insertions(+), 4 deletions(-) create mode 100644 lib/src/tank/collection.dart create mode 100644 lib/src/tank/drive.dart create mode 100644 lib/src/tank/gpio.dart diff --git a/lib/src/tank/collection.dart b/lib/src/tank/collection.dart new file mode 100644 index 0000000..f549afd --- /dev/null +++ b/lib/src/tank/collection.dart @@ -0,0 +1,36 @@ +import "package:burt_network/burt_network.dart"; + +import "drive.dart"; +import "gpio.dart"; + +class TankCollection extends Service { + late final server = RoverSocket(port: 8001, device: Device.SUBSYSTEMS, collection: this); + + final drive = TankDrive(); + final gpio = GpioService(); + + @override + Future init() async { + var result = true; + result &= await server.init(); + result &= await gpio.init(); + result &= await drive.init(); + return result; + } + + @override + Future onDisconnect() async { + drive.stop(); + await super.onDisconnect(); + } + + @override + Future dispose() async { + await drive.dispose(); + await gpio.dispose(); + } +} + +final tank = TankCollection(); + +final logger = BurtLogger(socket: tank.server); diff --git a/lib/src/tank/drive.dart b/lib/src/tank/drive.dart new file mode 100644 index 0000000..cd80752 --- /dev/null +++ b/lib/src/tank/drive.dart @@ -0,0 +1,58 @@ +import "dart:async"; + +import "package:autonomy/src/tank/collection.dart"; +import "package:burt_network/burt_network.dart"; + +class TankDrive extends Service { + StreamSubscription? _subscription; + + @override + Future init() async { + _subscription = tank.server.messages.onMessage( + name: DriveCommand().messageName, + constructor: DriveCommand.fromBuffer, + callback: _handleDriveCommand, + ); + return true; + } + + @override + Future onDisconnect() async { + stop(); + await super.onDisconnect(); + } + + @override + Future dispose() async { + await _subscription?.cancel(); + } + + void stop() { + _setThrottle(0); + _setSpeeds(left: 0, right: 0); + } + + void _setThrottle(double throttle) { + _throttle = throttle; + _update(); + } + + void _setSpeeds({required double left, required double right}) { + _leftSpeed = left; + _rightSpeed = right; + _update(); + } + + double _leftSpeed = 0; + double _rightSpeed = 0; + double _throttle = 0; + + void _handleDriveCommand(DriveCommand command) { + _leftSpeed = command.setLeft ? command.left : _leftSpeed; + _rightSpeed = command.setRight ? command.right : _rightSpeed; + _throttle = command.setThrottle ? command.throttle : _throttle; + _update(); + } + + void _update() => tank.gpio.updateDrive(left: _leftSpeed, right: _rightSpeed, throttle: _throttle); +} diff --git a/lib/src/tank/gpio.dart b/lib/src/tank/gpio.dart new file mode 100644 index 0000000..6bd7fb8 --- /dev/null +++ b/lib/src/tank/gpio.dart @@ -0,0 +1,55 @@ +import "package:burt_network/burt_network.dart"; +import "package:rpi_gpio/gpio.dart"; +import "package:rpi_gpio/rpi_gpio.dart"; + +class GpioService extends Service { + static const leftPinNumber = 13; + static const rightPinNumber = 14; + + static const minVelocity = -1.0; + static const maxVelocity = 1.0; + + static const pwmMaxPos = 82500.0; + static const pwmMinPos = 76500.0; + static const pwmZero = 75000.0; + static const pwmMinNeg = 73500.0; + static const pwmMaxNeg = 67500.0; + + late final RpiGpio gpio; + late final GpioPwm leftPin; + late final GpioPwm rightPin; + + @override + Future init() async { + try { + gpio = await initialize_RpiGpio(); + leftPin = gpio.pwm(leftPinNumber); + rightPin = gpio.pwm(rightPinNumber); + return true; + } on GpioException { + return false; + } + } + + @override + Future dispose() => gpio.dispose(); + + void updateDrive({required double left, required double right, required double throttle}) { + _setVelocity(leftPin, left * throttle); + _setVelocity(rightPin, right * 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(); + } +} diff --git a/pubspec.lock b/pubspec.lock index 0f65b3b..0855c24 100644 --- a/pubspec.lock +++ b/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: diff --git a/pubspec.yaml b/pubspec.yaml index c117b53..dcf82dd 100644 --- a/pubspec.yaml +++ b/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 From 1e80955206b3e747c4ec456a9b09deb672bbfe1c Mon Sep 17 00:00:00 2001 From: Levi Lesches Date: Wed, 13 Nov 2024 03:59:38 -0500 Subject: [PATCH 2/4] Consolidated all tank files to tank.dart --- lib/src/tank/collection.dart | 36 -------------- lib/src/tank/drive.dart | 58 ----------------------- lib/src/tank/gpio.dart | 55 ---------------------- lib/tank.dart | 91 ++++++++++++++++++++++++++++++++++++ 4 files changed, 91 insertions(+), 149 deletions(-) delete mode 100644 lib/src/tank/collection.dart delete mode 100644 lib/src/tank/drive.dart delete mode 100644 lib/src/tank/gpio.dart create mode 100644 lib/tank.dart diff --git a/lib/src/tank/collection.dart b/lib/src/tank/collection.dart deleted file mode 100644 index f549afd..0000000 --- a/lib/src/tank/collection.dart +++ /dev/null @@ -1,36 +0,0 @@ -import "package:burt_network/burt_network.dart"; - -import "drive.dart"; -import "gpio.dart"; - -class TankCollection extends Service { - late final server = RoverSocket(port: 8001, device: Device.SUBSYSTEMS, collection: this); - - final drive = TankDrive(); - final gpio = GpioService(); - - @override - Future init() async { - var result = true; - result &= await server.init(); - result &= await gpio.init(); - result &= await drive.init(); - return result; - } - - @override - Future onDisconnect() async { - drive.stop(); - await super.onDisconnect(); - } - - @override - Future dispose() async { - await drive.dispose(); - await gpio.dispose(); - } -} - -final tank = TankCollection(); - -final logger = BurtLogger(socket: tank.server); diff --git a/lib/src/tank/drive.dart b/lib/src/tank/drive.dart deleted file mode 100644 index cd80752..0000000 --- a/lib/src/tank/drive.dart +++ /dev/null @@ -1,58 +0,0 @@ -import "dart:async"; - -import "package:autonomy/src/tank/collection.dart"; -import "package:burt_network/burt_network.dart"; - -class TankDrive extends Service { - StreamSubscription? _subscription; - - @override - Future init() async { - _subscription = tank.server.messages.onMessage( - name: DriveCommand().messageName, - constructor: DriveCommand.fromBuffer, - callback: _handleDriveCommand, - ); - return true; - } - - @override - Future onDisconnect() async { - stop(); - await super.onDisconnect(); - } - - @override - Future dispose() async { - await _subscription?.cancel(); - } - - void stop() { - _setThrottle(0); - _setSpeeds(left: 0, right: 0); - } - - void _setThrottle(double throttle) { - _throttle = throttle; - _update(); - } - - void _setSpeeds({required double left, required double right}) { - _leftSpeed = left; - _rightSpeed = right; - _update(); - } - - double _leftSpeed = 0; - double _rightSpeed = 0; - double _throttle = 0; - - void _handleDriveCommand(DriveCommand command) { - _leftSpeed = command.setLeft ? command.left : _leftSpeed; - _rightSpeed = command.setRight ? command.right : _rightSpeed; - _throttle = command.setThrottle ? command.throttle : _throttle; - _update(); - } - - void _update() => tank.gpio.updateDrive(left: _leftSpeed, right: _rightSpeed, throttle: _throttle); -} diff --git a/lib/src/tank/gpio.dart b/lib/src/tank/gpio.dart deleted file mode 100644 index 6bd7fb8..0000000 --- a/lib/src/tank/gpio.dart +++ /dev/null @@ -1,55 +0,0 @@ -import "package:burt_network/burt_network.dart"; -import "package:rpi_gpio/gpio.dart"; -import "package:rpi_gpio/rpi_gpio.dart"; - -class GpioService extends Service { - static const leftPinNumber = 13; - static const rightPinNumber = 14; - - static const minVelocity = -1.0; - static const maxVelocity = 1.0; - - static const pwmMaxPos = 82500.0; - static const pwmMinPos = 76500.0; - static const pwmZero = 75000.0; - static const pwmMinNeg = 73500.0; - static const pwmMaxNeg = 67500.0; - - late final RpiGpio gpio; - late final GpioPwm leftPin; - late final GpioPwm rightPin; - - @override - Future init() async { - try { - gpio = await initialize_RpiGpio(); - leftPin = gpio.pwm(leftPinNumber); - rightPin = gpio.pwm(rightPinNumber); - return true; - } on GpioException { - return false; - } - } - - @override - Future dispose() => gpio.dispose(); - - void updateDrive({required double left, required double right, required double throttle}) { - _setVelocity(leftPin, left * throttle); - _setVelocity(rightPin, right * 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(); - } -} diff --git a/lib/tank.dart b/lib/tank.dart new file mode 100644 index 0000000..f0fbc2d --- /dev/null +++ b/lib/tank.dart @@ -0,0 +1,91 @@ +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; + +class Tank extends Service { + late final server = RoverSocket(port: 8004, device: Device.SUBSYSTEMS, collection: this); + + late RpiGpio gpio; + late GpioPwm leftPin; + late GpioPwm rightPin; + + double _leftSpeed = 0; + double _rightSpeed = 0; + double _throttle = 0; + + StreamSubscription? _subscription; + + @override + Future init() async { + try { + await server.init(); + _subscription = server.messages.onMessage( + name: DriveCommand().messageName, + constructor: DriveCommand.fromBuffer, + callback: _handleDriveCommand, + ); + gpio = await initialize_RpiGpio(); + leftPin = gpio.pwm(leftPinNumber); + rightPin = gpio.pwm(rightPinNumber); + return true; + } on GpioException { + return false; + } + } + + @override + Future dispose() async { + await _subscription?.cancel(); + await server.dispose(); + await gpio.dispose(); + } + + @override + Future onDisconnect() async { + _leftSpeed = 0; + _rightSpeed = 0; + _throttle = 0; + _updateMotors(); + await super.onDisconnect(); + } + + 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() { + _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(); + } +} From e630206c2c148f9161860626666cd0f5243c1972 Mon Sep 17 00:00:00 2001 From: Levi Lesches Date: Wed, 13 Nov 2024 04:14:05 -0500 Subject: [PATCH 3/4] Tank ready --- bin/tank.dart | 8 ++++++++ lib/tank.dart | 38 +++++++++++++++++++++++++++----------- 2 files changed, 35 insertions(+), 11 deletions(-) create mode 100644 bin/tank.dart diff --git a/bin/tank.dart b/bin/tank.dart new file mode 100644 index 0000000..1b2ede6 --- /dev/null +++ b/bin/tank.dart @@ -0,0 +1,8 @@ + + +import "package:autonomy/tank.dart"; + +void main() async { + final tank = Tank(); + await tank.init(); +} diff --git a/lib/tank.dart b/lib/tank.dart index f0fbc2d..89327d2 100644 --- a/lib/tank.dart +++ b/lib/tank.dart @@ -16,18 +16,23 @@ const pwmZero = 75000.0; const pwmMinNeg = 73500.0; const pwmMaxNeg = 67500.0; -class Tank extends Service { - late final server = RoverSocket(port: 8004, device: Device.SUBSYSTEMS, collection: this); +const dataInterval = Duration(milliseconds: 250); +final driveVersion = Version(major: 1, minor: 1); - late RpiGpio gpio; - late GpioPwm leftPin; - late GpioPwm rightPin; +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? _subscription; + Timer? _dataTimer; @override Future init() async { @@ -38,20 +43,23 @@ class Tank extends Service { constructor: DriveCommand.fromBuffer, callback: _handleDriveCommand, ); + _dataTimer = Timer.periodic(dataInterval, _sendData); gpio = await initialize_RpiGpio(); - leftPin = gpio.pwm(leftPinNumber); - rightPin = gpio.pwm(rightPinNumber); + leftPin = gpio?.pwm(leftPinNumber); + rightPin = gpio?.pwm(rightPinNumber); return true; - } on GpioException { + } catch (error) { + logger.critical("Could not initialize the Tank", body: error.toString()); return false; } } @override Future dispose() async { + _dataTimer?.cancel(); await _subscription?.cancel(); await server.dispose(); - await gpio.dispose(); + await gpio?.dispose(); } @override @@ -63,6 +71,13 @@ class Tank extends Service { 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; @@ -71,8 +86,9 @@ class Tank extends Service { } void _updateMotors() { - _setVelocity(leftPin, _leftSpeed * _throttle); - _setVelocity(rightPin, _rightSpeed * _throttle); + if (gpio == null) return; + _setVelocity(leftPin!, _leftSpeed * _throttle); + _setVelocity(rightPin!, _rightSpeed * _throttle); } void _setVelocity(GpioPwm pin, double speed) { From 6c9a1fb95ede9631f716b80da9d1041bcc1cdcd2 Mon Sep 17 00:00:00 2001 From: Levi Lesches Date: Wed, 13 Nov 2024 04:27:03 -0500 Subject: [PATCH 4/4] Fix lints --- bin/task.dart | 5 +---- bin/test.dart | 14 +++++++------- lib/src/interfaces/autonomy.dart | 1 - lib/src/interfaces/orchestrator.dart | 3 +-- lib/src/interfaces/reporter.dart | 2 +- lib/src/rover/drive/sensor.dart | 1 - lib/src/rover/imu.dart | 1 - 7 files changed, 10 insertions(+), 17 deletions(-) diff --git a/bin/task.dart b/bin/task.dart index a267658..6c26197 100644 --- a/bin/task.dart +++ b/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"); } diff --git a/bin/test.dart b/bin/test.dart index acae8bd..ac5a49e 100644 --- a/bin/test.dart +++ b/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(); } diff --git a/lib/src/interfaces/autonomy.dart b/lib/src/interfaces/autonomy.dart index bfd948f..ae69aba 100644 --- a/lib/src/interfaces/autonomy.dart +++ b/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) { diff --git a/lib/src/interfaces/orchestrator.dart b/lib/src/interfaces/orchestrator.dart index a1dc437..13ee8ed 100644 --- a/lib/src/interfaces/orchestrator.dart +++ b/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 init() async { - print("Orchestrator init 2"); collection.server.messages.onMessage( name: AutonomyCommand().messageName, constructor: AutonomyCommand.fromBuffer, diff --git a/lib/src/interfaces/reporter.dart b/lib/src/interfaces/reporter.dart index 05e848b..d15c983 100644 --- a/lib/src/interfaces/reporter.dart +++ b/lib/src/interfaces/reporter.dart @@ -13,7 +13,7 @@ mixin ValueReporter on Service { @override Future init() async { timer = Timer.periodic(reportInterval, (timer) => _reportValue()); - return await super.init(); + return super.init(); } @override diff --git a/lib/src/rover/drive/sensor.dart b/lib/src/rover/drive/sensor.dart index 7a6ea93..207e4fc 100644 --- a/lib/src/rover/drive/sensor.dart +++ b/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); diff --git a/lib/src/rover/imu.dart b/lib/src/rover/imu.dart index 8428ae5..ee0794f 100644 --- a/lib/src/rover/imu.dart +++ b/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; }