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/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; } diff --git a/lib/tank.dart b/lib/tank.dart new file mode 100644 index 0000000..89327d2 --- /dev/null +++ b/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? _subscription; + Timer? _dataTimer; + + @override + Future init() async { + try { + await server.init(); + _subscription = server.messages.onMessage( + 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 dispose() async { + _dataTimer?.cancel(); + await _subscription?.cancel(); + await server.dispose(); + await gpio?.dispose(); + } + + @override + Future 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(); + } +} 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