From 8c3aa7da78f132cd5ac0462af2db7105d9c16142 Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Fri, 24 Jan 2025 12:51:00 -0500 Subject: [PATCH 1/2] Improve Subsystems safety --- subsystems/lib/src/devices/firmware.dart | 101 ++++++++++++++++++++++- subsystems/lib/subsystems.dart | 9 +- 2 files changed, 100 insertions(+), 10 deletions(-) diff --git a/subsystems/lib/src/devices/firmware.dart b/subsystems/lib/src/devices/firmware.dart index 90e8bced..582c330e 100644 --- a/subsystems/lib/src/devices/firmware.dart +++ b/subsystems/lib/src/devices/firmware.dart @@ -27,12 +27,53 @@ final nameToDevice = { /// service is responsible for routing incoming UDP messages to the correct firmware device /// ([_sendToSerial]), and forwarding serial messages to the Dashboard ([RoverSocket.sendWrapper]). class FirmwareManager extends Service { + /// The amount of time to wait before automatically sending a + /// stop command to the firmware after not receiving messages + static const Duration firmwareStopTimeout = Duration(milliseconds: 500); + /// Subscriptions to each of the firmware devices. final List> _subscriptions = []; /// A list of firmware devices attached to the rover. List devices = []; + /// A map of devices to their timers to automatically shut + /// off if no new messages are received + /// + /// This prevents a scenario where commands aren't sent + /// from the dashboard, and that program disconnects, leaving + /// the motors to run freely + final Map deviceStopTimers = {}; + + /// The current status of the rover, as received from [UpdateSetting] + RoverStatus currentStatus = RoverStatus.MANUAL; + + /// Whether or not the firmware should be sent any messages, the firmware + /// should only be instructed to move when [currentStatus] is either [RoverStatus.AUTONOMOUS] + /// or [RoverStatus.MANUAL] + bool get shouldMove => + currentStatus == RoverStatus.AUTONOMOUS || + currentStatus == RoverStatus.MANUAL; + + StreamSubscription? _roverStatusSubscription; + + /// The command to stop the drive motors + final stopDrive = DriveCommand(throttle: 0, setThrottle: true); + /// The command to stop the arm + final stopArm = ArmCommand(stop: true); + /// The command to stop the gripper + final stopGripper = GripperCommand(stop: true); + /// The command to stop science + final stopScience = ScienceCommand(stop: true); + + /// Map of each firmware [Device] to its corresponding stop command + late final Map deviceToStopCommand = { + Device.DRIVE: stopDrive, + Device.ARM: stopArm, + Device.GRIPPER: stopGripper, + Device.SCIENCE: stopScience, + }; + @override Future init() async { devices = await getFirmwareDevices(); @@ -45,9 +86,30 @@ class FirmwareManager extends Service { final subscription = device.messages.listen(collection.server.sendWrapper); _subscriptions.add(subscription); } + currentStatus = RoverStatus.MANUAL; + + _roverStatusSubscription = collection.server.messages.onMessage( + name: UpdateSetting().messageName, + constructor: UpdateSetting.fromBuffer, + callback: onSettingsUpdate, + ); return result; } + /// Handles an incoming [UpdateSetting] message + void onSettingsUpdate(UpdateSetting setting) { + if (!setting.hasStatus()) return; + final status = setting.status; + + currentStatus = setting.status; + + if (status == RoverStatus.AUTONOMOUS) { + stopDevice(Device.DRIVE); + } else if (status != RoverStatus.MANUAL) { + stopHardware(); + } + } + @override Future dispose() async { for (final subscription in _subscriptions) { @@ -56,14 +118,38 @@ class FirmwareManager extends Service { for (final device in devices) { await device.dispose(); } + await _roverStatusSubscription?.cancel(); + } + + /// Sends a message to the firmware [device] to stop the device + void stopDevice(Device device) { + logger.debug("Stopping device $device"); + final stopCommand = deviceToStopCommand[device]; + if (stopCommand != null) { + sendMessage(stopCommand); + } } /// Sends a [WrappedMessage] to the correct Serial device. /// /// The notes on [sendMessage] apply here as well. - void _sendToSerial(WrappedMessage wrapper) { + void _sendToSerial(WrappedMessage wrapper, {bool fromNetwork = true}) { final device = nameToDevice[wrapper.name]; if (device == null) return; + deviceStopTimers[device]?.cancel(); + if (fromNetwork) { + if (!shouldMove) { + logger.debug("Ignoring incoming ${wrapper.name}, status is currently ${currentStatus.name}"); + return; + } + deviceStopTimers[device] = Timer(firmwareStopTimeout, () { + logger.info( + "Device timed out: $device", + body: "No commands have been received for ${firmwareStopTimeout.inMilliseconds} milliseconds", + ); + stopDevice(device); + }); + } final serial = devices.firstWhereOrNull((s) => s.device == device); if (serial == null) return; serial.sendBytes(wrapper.data); @@ -74,5 +160,16 @@ class FirmwareManager extends Service { /// This does nothing if the appropriate device is not connected. Specifically, this is not an /// error because the Dashboard may be used during testing, when the hardware devices may not be /// assembled, connected, or functional yet. - void sendMessage(Message message) => _sendToSerial(message.wrap()); + void sendMessage(Message message, {bool fromNetwork = false}) => + _sendToSerial(message.wrap(), fromNetwork: fromNetwork); + + /// Sends messages to all over the firmware devices to stop all movement + /// + /// This is used as a safety stop to prevent any damage to hardware + void stopHardware() { + stopDevice(Device.DRIVE); + stopDevice(Device.ARM); + stopDevice(Device.GRIPPER); + stopDevice(Device.SCIENCE); + } } diff --git a/subsystems/lib/subsystems.dart b/subsystems/lib/subsystems.dart index bb190f1e..43e8254e 100644 --- a/subsystems/lib/subsystems.dart +++ b/subsystems/lib/subsystems.dart @@ -80,14 +80,7 @@ class SubsystemsCollection extends Service { Future onDisconnect() async { await super.onDisconnect(); logger.info("Stopping all hardware"); - final stopDrive = DriveCommand(throttle: 0, setThrottle: true); - final stopArm = ArmCommand(stop: true); - final stopGripper = GripperCommand(stop: true); - final stopScience = ScienceCommand(stop: true); - firmware.sendMessage(stopDrive); - firmware.sendMessage(stopArm); - firmware.sendMessage(stopGripper); - firmware.sendMessage(stopScience); + firmware.stopHardware(); } /// Sends a [SubsystemsData] message over the network reporting the current status of subsystems From becdbe00613722ab96b82c2152795863833a200d Mon Sep 17 00:00:00 2001 From: Gold87 <91761103+Gold872@users.noreply.github.com> Date: Sun, 2 Feb 2025 01:22:30 -0500 Subject: [PATCH 2/2] Fix drive firmware's LED buttons - Listen to drive data to properly update the rover status - Send message to drive firmware to update the LEDs --- subsystems/lib/src/devices/firmware.dart | 47 +++++++++++++++++++++++- 1 file changed, 45 insertions(+), 2 deletions(-) diff --git a/subsystems/lib/src/devices/firmware.dart b/subsystems/lib/src/devices/firmware.dart index 582c330e..6b7a7acf 100644 --- a/subsystems/lib/src/devices/firmware.dart +++ b/subsystems/lib/src/devices/firmware.dart @@ -26,6 +26,12 @@ final nameToDevice = { /// class takes care of connecting to, identifying, and streaming from a firmware device. This /// service is responsible for routing incoming UDP messages to the correct firmware device /// ([_sendToSerial]), and forwarding serial messages to the Dashboard ([RoverSocket.sendWrapper]). +/// +/// This service also acts as the lowest level of sending commands to operate the rover, and where +/// the final "invalid" commands are filtered. The firmware manager maintains an internal [RoverStatus], +/// which is updated when either an [UpdateSetting] is received from the network, or a new [RoverStatus] is +/// reported from the drive firmware. Any commands that are received when the rover is not in the manual or +/// autonomous status will not be sent to the firmware, acting as the final safety barrier. class FirmwareManager extends Service { /// The amount of time to wait before automatically sending a /// stop command to the firmware after not receiving messages @@ -55,8 +61,12 @@ class FirmwareManager extends Service { currentStatus == RoverStatus.AUTONOMOUS || currentStatus == RoverStatus.MANUAL; + /// Subscription for the rover status message being sent over the network StreamSubscription? _roverStatusSubscription; + /// Subscription for the rover status message being sent from the drive firmware + StreamSubscription? _driveStatusSubscription; + /// The command to stop the drive motors final stopDrive = DriveCommand(throttle: 0, setThrottle: true); /// The command to stop the arm @@ -84,6 +94,13 @@ class FirmwareManager extends Service { result &= await device.init(); if (!device.isReady) continue; final subscription = device.messages.listen(collection.server.sendWrapper); + if (device.device == Device.DRIVE) { + _driveStatusSubscription = device.messages.onMessage( + name: DriveData().messageName, + constructor: DriveData.fromBuffer, + callback: onDriveData, + ); + } _subscriptions.add(subscription); } currentStatus = RoverStatus.MANUAL; @@ -96,12 +113,37 @@ class FirmwareManager extends Service { return result; } - /// Handles an incoming [UpdateSetting] message + /// Handles an incoming [DriveData] message + /// + /// This is only used to update [currentStatus] and the server based + /// on the Rover Status reported by the drive + void onDriveData(DriveData message) { + if (!message.hasStatus() || message.status == currentStatus) return; + + _onStatus(message.status); + + // Update the socket based on the setting, will also send the status to the dashboard + final updateSetting = UpdateSetting(status: message.status); + collection.server.onSettings(updateSetting); + } + + /// Handles an incoming [UpdateSetting] message from the network void onSettingsUpdate(UpdateSetting setting) { if (!setting.hasStatus()) return; final status = setting.status; - currentStatus = setting.status; + // Send status to drive to update the button LEDs + sendMessage(DriveCommand(status: status)); + + _onStatus(status); + } + + /// Handles an update to the Rover Status + /// + /// This will appropriately update [currentStatus] and if setting to idle, + /// send a stop command to appropriate firmware devices + void _onStatus(RoverStatus status) { + currentStatus = status; if (status == RoverStatus.AUTONOMOUS) { stopDevice(Device.DRIVE); @@ -119,6 +161,7 @@ class FirmwareManager extends Service { await device.dispose(); } await _roverStatusSubscription?.cancel(); + await _driveStatusSubscription?.cancel(); } /// Sends a message to the firmware [device] to stop the device