From 045237d44f3af090dd8bca54597701584f4739ea Mon Sep 17 00:00:00 2001 From: Dave Date: Sun, 3 Nov 2024 12:52:18 -0500 Subject: [PATCH] Initial restructuring after _actual_ spec added --- gem-remotes-esp32.code-workspace | 11 + gem-remotes-esp32/src/ble_server.rs | 140 +++- gem-remotes-esp32/src/main.rs | 15 +- gem-remotes-esp32/src/message_timer.rs | 2 + gem-remotes-esp32/src/motor_driver.rs | 70 -- gem-remotes-esp32/src/pair_button_driver.rs | 34 - gem-remotes-esp32/src/test_console.rs | 54 -- gem-remotes-lib/Cargo.toml | 1 + gem-remotes-lib/src/commands.rs | 55 +- gem-remotes-lib/src/fake_limits.rs | 62 ++ gem-remotes-lib/src/fake_motor.rs | 137 ++++ gem-remotes-lib/src/fake_pic.rs | 237 ++++++ gem-remotes-lib/src/fake_status.rs | 28 + gem-remotes-lib/src/lib.rs | 30 +- gem-remotes-lib/src/motor_controller.rs | 766 -------------------- 15 files changed, 659 insertions(+), 983 deletions(-) create mode 100644 gem-remotes-esp32.code-workspace delete mode 100644 gem-remotes-esp32/src/motor_driver.rs delete mode 100644 gem-remotes-esp32/src/pair_button_driver.rs create mode 100644 gem-remotes-lib/src/fake_limits.rs create mode 100644 gem-remotes-lib/src/fake_motor.rs create mode 100644 gem-remotes-lib/src/fake_pic.rs create mode 100644 gem-remotes-lib/src/fake_status.rs delete mode 100644 gem-remotes-lib/src/motor_controller.rs diff --git a/gem-remotes-esp32.code-workspace b/gem-remotes-esp32.code-workspace new file mode 100644 index 0000000..7b06e75 --- /dev/null +++ b/gem-remotes-esp32.code-workspace @@ -0,0 +1,11 @@ +{ + "folders": [ + { + "path": "gem-remotes-esp32" + }, + { + "path": "gem-remotes-lib" + } + ], + "settings": {} +} \ No newline at end of file diff --git a/gem-remotes-esp32/src/ble_server.rs b/gem-remotes-esp32/src/ble_server.rs index abdd83b..6f2c05f 100644 --- a/gem-remotes-esp32/src/ble_server.rs +++ b/gem-remotes-esp32/src/ble_server.rs @@ -1,6 +1,19 @@ +/* +BLE Server + +This module is responsible for: + - Setting up the bluetooth GAP profile + - Setting up the GATT characteristics + - Managing the advertising and pairing modes + - Taking writeable GATT characteristics and putting them in the event q + - taking status events from the q and putting them in the read GATT characteristics + + TODO: Consider splitting up these tasks. + +*/ use log::*; //{trace, debug, info, warn, error} -use esp32_nimble::{enums::*, uuid128, BLEAdvertisementData, BLEDevice, BLEServer, NimbleProperties, OnWriteArgs}; +use esp32_nimble::{enums::*, uuid128, BLEAdvertisementData, BLEDevice, BLEServer, DescriptorProperties, NimbleProperties, OnWriteArgs}; use esp32_nimble::utilities::BleUuid; use anyhow::Result; use closure::closure; @@ -14,6 +27,9 @@ use gem_remotes_lib::{ DispatchSendQ, Commands, Button, + EMPTY_MOTORS, + EMPTY_LIMITS, + Statuses, }; // TODO HARDWARE: test these values to see if they are suitable @@ -37,10 +53,26 @@ const DEVICE_NAME: &str = "Gem Remotes"; const UUID_SERVICE_PAIR: BleUuid = BleUuid::from_uuid128([0xB4, 0x09, 0x8D, 0xE0, 0x61, 0x08, 0x66, 0xBA, 0x61, 0x4B, 0x3C, 0xF1, 0x5A, 0xAD, 0x66, 0x99]); const UUID_SERVICE_LIFT: BleUuid = uuid128!("c1400000-8dda-45a3-959b-d23a0f8f53d7"); +// Command Characteristics const UUID_BUTTON_UP: BleUuid = uuid128!("c1401121-8dda-45a3-959b-d23a0f8f53d7"); const UUID_BUTTON_DOWN: BleUuid = uuid128!("c1401122-8dda-45a3-959b-d23a0f8f53d7"); const UUID_BUTTON_STOP: BleUuid = uuid128!("c1401123-8dda-45a3-959b-d23a0f8f53d7"); -const UUID_BLUETOOTH_NAME: BleUuid = uuid128!("c1401224-8dda-45a3-959b-d23a0f8f53d7"); +const UUID_BUTTON_AUX: BleUuid = uuid128!("c1401124-8dda-45a3-959b-d23a0f8f53d7"); +const UUID_BUTTON_LEARN: BleUuid = uuid128!("c1401223-8dda-45a3-959b-d23a0f8f53d7"); +const UUID_BUTTON_AUTO: BleUuid = uuid128!("c1401225-8dda-45a3-959b-d23a0f8f53d7"); +const UUID_BLUETOOTH_NAME: BleUuid = uuid128!("c1411224-8dda-45a3-959b-d23a0f8f53d7"); + +// Status Characteristics +const UUID_STATUS_LIMITS: BleUuid = uuid128!("c1401321-8dda-45a3-959b-d23a0f8f53d7"); +const UUID_STATUS_MOTOR: BleUuid = uuid128!("c1401322-8dda-45a3-959b-d23a0f8f53d7"); +const UUID_STATUS_STATUS: BleUuid = uuid128!("c1401323-8dda-45a3-959b-d23a0f8f53d7"); +const UUID_STATUS_REASON: BleUuid = uuid128!("c1401324-8dda-45a3-959b-d23a0f8f53d7"); + +// Expanded Services +const UUID_EXPANDED_SSID: BleUuid = uuid128!("c1411221-8dda-45a3-959b-d23a0f8f53d7"); //IMPLIMENT +const UUID_EXPANDED_WIFI_PASS: BleUuid = uuid128!("c1411222-8dda-45a3-959b-d23a0f8f53d7"); //IMPLIMENT +const UUID_EXPANDED_LOG_TRIGGER: BleUuid = uuid128!("c1401421-8dda-45a3-959b-d23a0f8f53d7"); //IMPLIMENT +const UUID_EXPANDED_LOG_DATA: BleUuid = uuid128!("c1401422-8dda-45a3-959b-d23a0f8f53d7"); //IMPLIMENT const BLE_BUTTON_RELEASE: u8 = 0; const BLE_BUTTON_PRESS: u8 = 1; @@ -56,13 +88,15 @@ pub struct BleServer { impl BleServer { pub fn new(dp: &mut Dispatch) -> Self { let cmds = vec![ - Commands::NotifyMotorDown { data: Button::Released }, - Commands::NotifyMotorStop { data: Button::Released }, - Commands::NotifyMotorUp { data: Button::Released }, Commands::PairTimerExpired, Commands::AllowPairing, Commands::EraseBleBonds, Commands::BluetoothName { data: Arc::new(String::new())}, + Commands::BluetoothStatusLimits { data: EMPTY_LIMITS }, + Commands::BluetoothStatusMotor { data: EMPTY_MOTORS }, + Commands::BluetoothStatusStatus { data: Statuses::empty() }, + Commands::BluetoothStatusReason { data: Arc::::new("".to_string()) }, + //TODONOW! We need to add the status commands here! ]; let r = dp.get_callback_channel(&cmds); let s = dp.get_cmd_channel(); @@ -96,16 +130,18 @@ impl BleServer { // --- Button Up Bluetooth GATT ---------------------------------------------------------- let button_up = lift_service.lock().create_characteristic( UUID_BUTTON_UP, - NimbleProperties::READ | NimbleProperties::WRITE_NO_RSP | NimbleProperties::NOTIFY, + NimbleProperties::WRITE, ); button_up.lock().set_value(&[0]) .on_write(closure!(clone sender, |args: &mut OnWriteArgs| { on_bluetooth_cmd(&sender, args, Commands::BluetoothUp {data: Button::Released}) })); + let button_up_name = button_up.lock().create_descriptor(BleUuid::Uuid16(0x2901), DescriptorProperties::READ); + button_up_name.lock().set_value(b"Command Up"); // --- Button Down Bluetooth GATT -------------------------------------------------------- let button_down = lift_service.lock().create_characteristic( UUID_BUTTON_DOWN, - NimbleProperties::READ | NimbleProperties::WRITE_NO_RSP | NimbleProperties::NOTIFY, + NimbleProperties::WRITE, ); button_down.lock().set_value(&[0]) .on_write(closure!(clone sender, |args: &mut OnWriteArgs| { @@ -114,21 +150,69 @@ impl BleServer { // --- Button Stop Bluetooth GATT -------------------------------------------------------- let button_stop = lift_service.lock().create_characteristic( UUID_BUTTON_STOP, - NimbleProperties::READ | NimbleProperties::WRITE_NO_RSP | NimbleProperties::NOTIFY, + NimbleProperties::WRITE, ); button_stop.lock().set_value(&[1]) .on_write(closure!(clone sender, |args: &mut OnWriteArgs| { on_bluetooth_cmd(&sender, args, Commands::BluetoothStop {data: Button::Released}) })); + // --- Button Aux Bluetooth GATT -------------------------------------------------------- + let button_aux = lift_service.lock().create_characteristic( + UUID_BUTTON_AUX, + NimbleProperties::WRITE, + ); + button_aux.lock().set_value(&[0]) + .on_write(closure!(clone sender, |args: &mut OnWriteArgs| { + on_bluetooth_cmd(&sender, args, Commands::BluetoothAux {data: Button::Released}) + })); + // --- Button Learn Bluetooth GATT -------------------------------------------------------- + let button_learn = lift_service.lock().create_characteristic( + UUID_BUTTON_LEARN, + NimbleProperties::WRITE | NimbleProperties::READ, + ); + button_learn.lock().set_value(&[0]) + .on_write(closure!(clone sender, |args: &mut OnWriteArgs| { + on_bluetooth_cmd(&sender, args, Commands::BluetoothLearn {data: Button::Released}) + })); + // --- Button Auto Bluetooth GATT -------------------------------------------------------- + let button_auto = lift_service.lock().create_characteristic( + UUID_BUTTON_AUTO, + NimbleProperties::WRITE, + ); + button_auto.lock().set_value(&[0]) + .on_write(closure!(clone sender, |args: &mut OnWriteArgs| { + on_bluetooth_cmd(&sender, args, Commands::BluetoothAuto {data: Button::Released}) + })); // --- Device Name Bluetooth GATT -------------------------------------------------------- let device_name = lift_service.lock().create_characteristic( UUID_BLUETOOTH_NAME, - NimbleProperties::READ | NimbleProperties::WRITE, + NimbleProperties::WRITE | NimbleProperties::READ, ); device_name.lock().set_value(self.dev_name.as_bytes()) .on_write(closure!(clone sender, |args: &mut OnWriteArgs| { on_bluetooth_cmd(&sender, args, Commands::BluetoothName { data: Arc::new(String::new())}.clone()) })); + // --- Status Limits Bluetooth GATT -------------------------------------------------------- + let status_limits = lift_service.lock().create_characteristic( + UUID_STATUS_LIMITS, + NimbleProperties::READ | NimbleProperties::INDICATE, + ); + // --- Status Motor Bluetooth GATT -------------------------------------------------------- + let status_motor = lift_service.lock().create_characteristic( + UUID_STATUS_MOTOR, + NimbleProperties::READ | NimbleProperties::INDICATE, + ); + // --- Status Status Bluetooth GATT -------------------------------------------------------- + let status_status = lift_service.lock().create_characteristic( + UUID_STATUS_STATUS, + NimbleProperties::READ | NimbleProperties::INDICATE, + ); + // --- Status Reason Bluetooth GATT -------------------------------------------------------- + let status_reason = lift_service.lock().create_characteristic( + UUID_STATUS_REASON, + NimbleProperties::READ | NimbleProperties::INDICATE, + ); + // Default to not pairable self.advertise_unpairable()?; @@ -137,23 +221,29 @@ impl BleServer { let cmd =self.recv_q.recv().await.expect("Bluetooth notification queue unexpectedly closed"); trace!("Received update to bluetooth variable {:?}", cmd); match cmd { - // TODO DISCUSS: This logic (if one button is pressed others are released) could be done in app instead. - Commands::NotifyMotorUp{data} => { - button_up.lock().set_value(&button_to_ble_button(data)).notify(); + // Handle events that require bluetooth, such as status and pairing changes + + //Status changes to notify app on: + Commands::BluetoothStatusLimits{data} => { + status_limits.lock().set_value(&data.as_bytes()).notify(); // Also handles indicate } - Commands::NotifyMotorDown{data} => { - button_down.lock().set_value(&button_to_ble_button(data)).notify(); + Commands::BluetoothStatusMotor{data} => { + status_motor.lock().set_value(&data.as_bytes()).notify(); // Also handles indicate } - Commands::NotifyMotorStop{data} => { - button_up.lock().set_value(&button_to_ble_button(data)).notify(); + Commands::BluetoothStatusStatus{data} => { + status_status.lock().set_value(&data.to_le_bytes()).notify(); // Also handles indicate } + Commands::BluetoothStatusReason{data} => { + status_reason.lock().set_value(data.as_str().as_bytes()).notify(); // Also handles indicate + } + Commands::PairTimerExpired => { self.advertise_unpairable()?; self.send_q.send(Commands::PairTimerClear).await?; } Commands::AllowPairing => { self.advertise_pairable()?; - debug!("pairing mode / non-pairing mode not currently supported"); + debug!("pairing mode / non-pairing mode not currently supported"); //TODO Really? Didn't we just test this? } Commands::EraseBleBonds => { ble_device.delete_all_bonds().expect("Failed trying to erase bluetooth bonding information"); @@ -233,6 +323,15 @@ fn on_bluetooth_cmd(sender: &DispatchSendQ, args: &mut OnWriteArgs, cmd: Command sender.send_blocking(Commands::BluetoothName { data: Arc::new(String::from_str(DEVICE_NAME).unwrap()) }) } } + Commands::BluetoothAuto { data: _ } => { + sender.send_blocking(Commands::BluetoothAuto { data: ble_to_button(v) }) + } + Commands::BluetoothAux { data: _ } => { + sender.send_blocking(Commands::BluetoothAux { data: ble_to_button(v) }) + } + Commands::BluetoothLearn { data: _ } => { + sender.send_blocking(Commands::BluetoothLearn { data: ble_to_button(v) }) + } //TODO when we get name changes, truncate to 31 chars, because that's what we have anyway. _ => {error!("Tried to handle an unknown bluetooth command: {:?}",cmd);Ok(())} }; @@ -290,12 +389,5 @@ fn set_server_callbacks(server: &mut BLEServer, sender: DispatchSendQ) { }); } -fn button_to_ble_button(but: Button) -> [u8; 1] { - match but { - Button::Released => {[BLE_BUTTON_RELEASE]} - Button::Pressed => {[BLE_BUTTON_PRESS]} - } -} - //TODO set maximum pairs to remember? //TODO after disconnect, it returns to scanning - will it return to directed scanning? Find out when directed is working. \ No newline at end of file diff --git a/gem-remotes-esp32/src/main.rs b/gem-remotes-esp32/src/main.rs index fbbccc3..8713d10 100644 --- a/gem-remotes-esp32/src/main.rs +++ b/gem-remotes-esp32/src/main.rs @@ -1,5 +1,5 @@ const BUTTON_HOLD_TIME_MS: u64 = 1_200; -const STOP_SAFETY_TIME_MS: u64 = 2_000; +const STOP_SAFETY_TIME_MS: u64 = 3_000; const PAIR_TIME_MS: u64 = 30_000; // Crates used in release @@ -12,7 +12,7 @@ use std::ops::Deref; use gem_remotes_lib::{ Commands, - Controller, + FakePic, Dispatch, }; @@ -20,10 +20,8 @@ use gem_remotes_lib::{ mod test_console; // Release modules -mod motor_driver; mod message_timer; mod ble_server; -mod pair_button_driver; use crate::message_timer::MessageTimer; //use crate::commands::Commands; @@ -72,12 +70,8 @@ async fn main_loop() -> Result<()> { // Create dispatch early so it can outlive most other things let mut dp = Dispatch::new(); - // Debug Drivers (TODO DEBUG: remove debug) - let motor_driver = motor_driver::MotorDriverDebug::new(); - // Setup of various drivers that need to out-live the executor - let m_chan = Controller::prepare_controller(&mut dp); - let mut motor_control = Controller::new(m_chan, dp.get_cmd_channel(), motor_driver.get_endpoint()); + let mut f_pic = FakePic::with_defaults(&mut dp); // Setup callback timers let mut button_timer = MessageTimer::::new_on_dispatch( Commands::ButtonTimerRestart, @@ -107,12 +101,11 @@ async fn main_loop() -> Result<()> { tasks.push(executor.spawn(test_console::start_cli(cli_endpoint))); // Queueu up our async tasks - tasks.push(executor.spawn(motor_control.run())); tasks.push(executor.spawn(button_timer.run())); tasks.push(executor.spawn(stopping_timer.run())); tasks.push(executor.spawn(pairing_timer.run())); tasks.push(executor.spawn(ble_server.run())); - tasks.push(executor.spawn(motor_driver.run())); + tasks.push(executor.spawn(f_pic.run())); tasks.push(executor.spawn(dp.cmd_loop())); //Once we have all our tasks, await on them all to run them in parallel. diff --git a/gem-remotes-esp32/src/message_timer.rs b/gem-remotes-esp32/src/message_timer.rs index 02785ea..40a140a 100644 --- a/gem-remotes-esp32/src/message_timer.rs +++ b/gem-remotes-esp32/src/message_timer.rs @@ -30,6 +30,7 @@ pub struct MessageTimer { } impl MessageTimer { + /* pub fn new( restart: Q, cancel: Q, @@ -48,6 +49,7 @@ impl MessageTimer { state: State::Stopped, } } + */ pub fn new_on_dispatch( restart: Commands, diff --git a/gem-remotes-esp32/src/motor_driver.rs b/gem-remotes-esp32/src/motor_driver.rs deleted file mode 100644 index bf8e4af..0000000 --- a/gem-remotes-esp32/src/motor_driver.rs +++ /dev/null @@ -1,70 +0,0 @@ -/// Handles the actual hardware interface with motor or its controller. - -use log::*; //{trace, debug, info, warn, error} -use anyhow::Result; -use async_channel::unbounded; -use gem_remotes_lib::{MotorCommands, MotorRecvQ, MotorSendQ}; - - -pub struct MotorDriverDebug{ - endpoint: MotorSendQ, // Endpoint to hand to dispatch or anyone else sending commands here. - recv_q: MotorRecvQ, -} - -/// Debug / example version of Motor Driver. -impl MotorDriverDebug { - pub fn new() -> Self { - let (s,r) = unbounded(); // TODO: reserve a reasonable amount for all unbounded? - MotorDriverDebug { - endpoint: s, - recv_q: r, - } - } - - pub fn get_endpoint(&self) -> MotorSendQ { - self.endpoint.clone() - } - - pub async fn run(&self) -> Result<()> { - loop { - let cmd = self.recv_q.recv() - .await - .expect("Unexpected failure in motor driver command queue"); - self.handle_cmd(cmd) - .await - .expect("Unexpected failure of motor driver notification queue"); - } - } - - async fn handle_cmd(&self, cmd: MotorCommands) -> Result<()> { - match cmd { - MotorCommands::StartUp => {self.start_up().await?;} - MotorCommands::StartDown => {self.start_down().await?;} - MotorCommands::Stop => {self.stop().await?;} - } - Ok(()) - } - - async fn start_up(&self) -> Result<()> { - warn!("Starting motor, direction: Up"); - Ok(()) - } - async fn start_down(&self) -> Result<()> { - warn!("Starting motor, direction: Down"); - Ok(()) - } - async fn stop(&self) -> Result<()> { - warn!("Stopping motor"); - Ok(()) - } - -} - - -//TODO: we should fix panic to ensure that we shut down motors before rebooting ESP! -// Maybe by getting another endpoint and passing it to the panic handler? Add a different -// command that doesn't just stop, but stops and stops processing any new commands. - -//TODO: Design - are there any implications to the PIC motor driver essentially sending button -// presses instead of commanding the motor on/off? Feedback loops? No way to know without PIC -// code. \ No newline at end of file diff --git a/gem-remotes-esp32/src/pair_button_driver.rs b/gem-remotes-esp32/src/pair_button_driver.rs deleted file mode 100644 index 7a2fb29..0000000 --- a/gem-remotes-esp32/src/pair_button_driver.rs +++ /dev/null @@ -1,34 +0,0 @@ -use log::*; //{trace, debug, info, warn, error} -use anyhow::Result; -use async_channel::Sender; -use esp_idf_svc::timer::EspTaskTimerService; -use core::time::Duration; - -use gem_remotes_lib::{ - Commands, - Dispatch, -}; - -type SendQ = Sender; - -pub struct PairButtonDriver { - _send: SendQ -} - -impl PairButtonDriver { - pub fn new(dp: &mut Dispatch) -> Self { - let s = dp.get_cmd_channel(); - Self { _send: s } - } - - pub async fn run(&self) -> Result<()> { - let timer_service = EspTaskTimerService::new()?; - let mut async_timer = timer_service.timer_async()?; - debug!("Waiting on pairing button presses"); - loop { - //TO DO: Watch for incoming pair button presses from the PIC and/or hardware buttons - async_timer.after(Duration::from_millis(10_000)).await?; // no need to panic on test console driver timer failure - //When we find a press, send PicRecvPair - } - } -} \ No newline at end of file diff --git a/gem-remotes-esp32/src/test_console.rs b/gem-remotes-esp32/src/test_console.rs index 3ed3250..0756809 100644 --- a/gem-remotes-esp32/src/test_console.rs +++ b/gem-remotes-esp32/src/test_console.rs @@ -35,26 +35,6 @@ use gem_remotes_lib::{ #[derive(Command)] pub enum Menu{//<'a> { - /// Simulate the PIC controller sending aus n Up character - PicRecvUp { - /// 0 for not pressed, 1 for pressed - data: u8, - }, - - /// Simulate the PIC controller sending us a Down character - PicRecvDown { - /// 0 for not pressed, 1 for pressed - data: u8, - }, - - /// Simulate the PIC controller sending us a Stop character - PicRecvStop { - /// 0 for not pressed, 1 for pressed - data: u8, - }, - - /// Simulate the PIC controller sending a "pair" button press - PicRecvPair, /// Send a bluetooth characteristic: Up BluetoothUp { @@ -121,40 +101,6 @@ pub fn process_menu( match command { // We ignore sending errors throughout because the Cli interface is only for // testing and debugging. - Menu::PicRecvUp {data} => { - let but = input_to_button(data); - match but { - Some(d) => { - println!("Sending PicRecvUp command"); - let _ = dispatch.send_blocking(Commands::PicRecvUp{data: d}); - } - None => {println!("Incorrect value; enter 0 or 1")} - } - } - Menu::PicRecvDown{data} => { - let but = input_to_button(data); - match but { - Some(d) => { - println!("Sending PicRecvUp command"); - let _ = dispatch.send_blocking(Commands::PicRecvDown{data: d}); - } - None => {println!("Incorrect value; enter 0 or 1")} - } - } - Menu::PicRecvStop{data} => { - let but = input_to_button(data); - match but { - Some(d) => { - println!("Sending PicRecvUp command"); - let _ = dispatch.send_blocking(Commands::PicRecvStop{data: d}); - } - None => {println!("Incorrect value; enter 0 or 1")} - } - } - Menu::PicRecvPair => { - cli.writer().write_str("Sending PIC command and timer reset (the job of the pair button driver, once PIC interface exists)")?; //TODO: PIC get this. - let _ = dispatch.send_blocking(Commands::AllowPairing); - } Menu::BluetoothUp { data } => { let but = input_to_button(data); match but { diff --git a/gem-remotes-lib/Cargo.toml b/gem-remotes-lib/Cargo.toml index c39a7b3..5258ff4 100644 --- a/gem-remotes-lib/Cargo.toml +++ b/gem-remotes-lib/Cargo.toml @@ -15,6 +15,7 @@ edition = "2021" anyhow = "1.0.86" async-channel = "2.3.1" async-io = "2.3.4" +bitflags = "2.6.0" log = "0.4.22" strum = "0.26.3" strum_macros = "0.26.4" diff --git a/gem-remotes-lib/src/commands.rs b/gem-remotes-lib/src/commands.rs index ffb7568..5c89b66 100644 --- a/gem-remotes-lib/src/commands.rs +++ b/gem-remotes-lib/src/commands.rs @@ -4,6 +4,10 @@ use strum_macros::EnumCount as EnumCountMacro; use std::mem::discriminant; use std::sync::Arc; +use crate::fake_limits::Limits; +use crate::fake_motor::Motors; +use crate::fake_status::Statuses; + #[derive(Clone, EnumCountMacro, Debug)] pub enum Commands { // Use Arc for any data larger than 4 bytes, to keep message passing queue size down. @@ -11,50 +15,73 @@ pub enum Commands { // Inputs sent from the PIC microcontroller // TODO: move these to buttons driver, for eventual move to ESP only? - // PIC button commands are considered "remote" due to the delay in sending notification - PicRecvUp {data: Button}, - PicRecvDown {data: Button}, - PicRecvStop {data: Button}, - PicRecvLimitUp {data: Button}, - PicRecvLimitDown {data: Button}, - PicRecvAutoMode {data: Toggle}, // 0 for disallowed, 1 for allowed + // Commands to Fake PIC from console + FPicToggleAux, + FPicToggleAuto, + FPicPressLearn, + FPicPressUp, + FPicPressDown, + FPicPressStop, // Releasing up or down should be equivalent to stop, as motors can only go in one direction at a time. + FPicTogglePanic, + FPicLockout, + FPicFault {data: Arc}, // String is cause. Send empty string to clear fault. + FPicLimit {data: Limits}, // TODO: real hardware buttons - consider re-sending occasionally when pressed, so that transitions like holding up -> stopping -> holding down -> stopped -> (should go down but gets no new notice) work. - // Inputs from bluetooth - BluetoothUp {data: Button}, //TODO change these to real button states and change them on input. + // Inputs via bluetooth (Recv from app) + BluetoothUp {data: Button}, BluetoothDown {data: Button}, - BluetoothStop {data: Button}, // There is no state where releasing the stop button induces a change. + BluetoothStop {data: Button}, + BluetoothAux {data: Button}, + BluetoothAuto {data: Button}, + BluetoothLearn {data: Button}, BluetoothName {data: Arc}, - //TODO: Allow auto mode to be set via bluetooth as well + // TODONOW: Do we have a separate panic command? Very hard to send up,down,and stop 'simultaneously' from app + + // Status (Send to app) + BluetoothStatusLimits {data: Limits}, + BluetoothStatusMotor {data: Motors}, + BluetoothStatusStatus {data: Statuses}, + BluetoothStatusReason {data: Arc}, + // Internal messages StopTimerExpired, // Sent when the 2 second stop sequence is complete StopTimerRestart, StopTimerClear, - ButtonTimerExpired, // TODO: these won't be necessary for hardware buttons; rename to PIC timer? + ButtonTimerExpired, // TODO: these won't be necessary for hardware buttons; rename to PIC timer? ButtonTimerRestart, ButtonTimerClear, PairTimerExpired, AllowPairing, // Also serves as the timer restart command PairTimerClear, + /* We no longer use these; delete! NotifyMotorUp {data: Button}, NotifyMotorDown {data: Button}, NotifyMotorStop {data: Button}, + */ EraseBleBonds, TestingExit, // Used only in unit/integration tests. Do not subscribe for. } -#[derive(Copy, Clone, Debug)] +#[derive(Copy, Clone, Debug, PartialEq)] pub enum Button { Released = 0, Pressed =1 } +impl Button { + pub fn is_pressed(self) -> bool { + self == Button::Pressed + } +} + // Distinguish toggles(like auto) which is on/off from buttons (which are pressed/released) +//TODONOW: remove? #[derive(Copy, Clone, Debug)] pub enum Toggle { Inactive = 0, @@ -68,4 +95,4 @@ impl PartialEq for Commands { fn eq(&self, other: &Self) -> bool { discriminant(self) == discriminant(other) } -} \ No newline at end of file +} diff --git a/gem-remotes-lib/src/fake_limits.rs b/gem-remotes-lib/src/fake_limits.rs new file mode 100644 index 0000000..08d66c6 --- /dev/null +++ b/gem-remotes-lib/src/fake_limits.rs @@ -0,0 +1,62 @@ +/// Fake Limit switch states for simulation. +/// + +#[derive(Copy, Clone, Debug)] +pub enum LimitStatus { + NotActive = 0, + BottomActive, + TopActive, + NotPresent = -1, +} + +impl LimitStatus { + pub fn to_u8(self) -> u8 { + match self { + LimitStatus::NotActive => {0} + LimitStatus::BottomActive => {1} + LimitStatus::TopActive =>{2} + LimitStatus::NotPresent => {25} + } + } + pub fn from_u8(i: u8) -> LimitStatus { + match i { + 0 => {LimitStatus::NotActive} + 1 => {LimitStatus::BottomActive} + 2 => {LimitStatus::TopActive} + _ => {LimitStatus::NotPresent} // Interpret bad data as missing limit + } + } +} + +#[derive(Copy, Clone, Debug)] +pub struct Limits { + pub only_starboard_bow: LimitStatus, + pub port_bow: LimitStatus, + pub starboard_quarter: LimitStatus, + pub port_quarter: LimitStatus, + +} + +impl Limits { + pub fn as_bytes(&self) -> [u8; 4] { + [self.only_starboard_bow.to_u8(), + self.port_bow.to_u8(), + self.starboard_quarter.to_u8(), + self.port_quarter.to_u8()] + } + pub fn from_bytes(b: [u8; 4]) -> Limits { + Limits{ + only_starboard_bow:LimitStatus::from_u8(b[0]), + port_bow:LimitStatus::from_u8(b[1]), + starboard_quarter:LimitStatus::from_u8(b[2]), + port_quarter:LimitStatus::from_u8(b[3]), + } + } +} + +pub const EMPTY_LIMITS: Limits = Limits{ + only_starboard_bow: LimitStatus::NotPresent, + port_bow: LimitStatus::NotPresent, + starboard_quarter: LimitStatus::NotPresent, + port_quarter: LimitStatus::NotPresent, +}; \ No newline at end of file diff --git a/gem-remotes-lib/src/fake_motor.rs b/gem-remotes-lib/src/fake_motor.rs new file mode 100644 index 0000000..67528c2 --- /dev/null +++ b/gem-remotes-lib/src/fake_motor.rs @@ -0,0 +1,137 @@ +/// Fake motor for simulation and testing +/// + + +#[derive(Copy, Clone, Debug, PartialEq)] +pub enum MotorStatus { + Stopped = 0, + GoingUp, + GoingDown, + Cooldown, + NotPresent = -1, +} + +impl MotorStatus { + pub fn to_u8(self) -> u8 { + match self { + MotorStatus::Stopped => {0} + MotorStatus::GoingUp => {1} + MotorStatus::GoingDown => {2} + MotorStatus::Cooldown => {3} + MotorStatus::NotPresent => {255} + } + } + pub fn from_u8(i: u8) -> MotorStatus { + match i { + 0 => {MotorStatus::Stopped} + 1 => {MotorStatus::GoingUp} + 2 => {MotorStatus::GoingDown} + 3 => {MotorStatus::Cooldown} + _ => {MotorStatus::NotPresent} // Assume bad data is no motor + } + } +} + +#[derive(Copy, Clone, Debug)] +pub struct Motors { + pub only_starboard_bow: MotorStatus, + pub port_bow: MotorStatus, + pub starboard_quarter: MotorStatus, + pub port_quarter: MotorStatus, + +} + +impl Motors { + pub fn update_status(&mut self, status: MotorStatus) -> () { + if self.only_starboard_bow != MotorStatus::NotPresent { + self.only_starboard_bow = status; + } + if self.port_bow != MotorStatus::NotPresent { + self.port_bow = status; + } + if self.starboard_quarter != MotorStatus::NotPresent { + self.starboard_quarter = status; + } + if self.port_quarter != MotorStatus::NotPresent { + self.port_quarter = status; + } + } + pub fn as_bytes(&self) -> [u8; 4] { + [self.only_starboard_bow.to_u8(), + self.port_bow.to_u8(), + self.starboard_quarter.to_u8(), + self.port_quarter.to_u8()] + } + pub fn from_bytes(b: [u8; 4]) -> Motors { + Motors{ + only_starboard_bow: MotorStatus::from_u8(b[0]), + port_bow: MotorStatus::from_u8(b[1]), + starboard_quarter: MotorStatus::from_u8(b[2]), + port_quarter: MotorStatus::from_u8(b[3]), + } + } + pub fn going_up(&self) -> bool { + // Since this is just simulation code, there is no need to check for + // complex and disallowed states. + self.only_starboard_bow == MotorStatus::GoingUp || + self.port_bow == MotorStatus::GoingUp || + self.starboard_quarter == MotorStatus::GoingUp || + self.port_quarter == MotorStatus::GoingUp + } + pub fn going_down(&self) -> bool { + // Since this is just simulation code, there is no need to check for + // complex and disallowed states. + self.only_starboard_bow == MotorStatus::GoingDown || + self.port_bow == MotorStatus::GoingDown || + self.starboard_quarter == MotorStatus::GoingDown || + self.port_quarter == MotorStatus::GoingDown + } + pub fn are_cooldown(&self) -> bool { + // Since this is just simulation code, there is no need to check for + // complex and disallowed states. + self.only_starboard_bow == MotorStatus::Cooldown || + self.port_bow == MotorStatus::Cooldown || + self.starboard_quarter == MotorStatus::Cooldown || + self.port_quarter == MotorStatus::Cooldown + } + pub fn are_stopped(&self) -> bool { + // Since this is just simulation code, there is no need to check for + // complex and disallowed states. + !(self.going_up() || self.going_down()) + } + pub fn finish_cooldown(&mut self) -> () { + if self.only_starboard_bow == MotorStatus::Cooldown { + self.only_starboard_bow = MotorStatus::Stopped + } + if self.port_bow == MotorStatus::Cooldown { + self.port_bow = MotorStatus::Stopped + } + if self.starboard_quarter == MotorStatus::Cooldown { + self.starboard_quarter = MotorStatus::Stopped + } + if self.port_quarter == MotorStatus::Cooldown { + self.port_quarter = MotorStatus::Stopped + } + } + pub fn stop(&mut self) -> () { + if self.only_starboard_bow == MotorStatus::GoingDown || self.only_starboard_bow == MotorStatus::GoingUp { + self.only_starboard_bow = MotorStatus::Stopped; + } + if self.port_bow == MotorStatus::GoingDown || self.port_bow == MotorStatus::GoingUp { + self.port_bow = MotorStatus::Stopped; + } + if self.starboard_quarter == MotorStatus::GoingDown || self.starboard_quarter == MotorStatus::GoingUp { + self.starboard_quarter = MotorStatus::Stopped; + } + if self.port_quarter == MotorStatus::GoingDown || self.port_quarter == MotorStatus::GoingUp { + self.port_quarter = MotorStatus::Stopped; + } + } +} + +pub const EMPTY_MOTORS: Motors = Motors{ + only_starboard_bow: MotorStatus::NotPresent, + port_bow: MotorStatus::NotPresent, + starboard_quarter: MotorStatus::NotPresent, + port_quarter: MotorStatus::NotPresent, +}; \ No newline at end of file diff --git a/gem-remotes-lib/src/fake_pic.rs b/gem-remotes-lib/src/fake_pic.rs new file mode 100644 index 0000000..8508a33 --- /dev/null +++ b/gem-remotes-lib/src/fake_pic.rs @@ -0,0 +1,237 @@ +/// Emulate the PIC controller for testing purposes +/// +/// This does not aim to be an accurate representation of the logic on the PIC, +/// and there are several things it does not handle (eg RF remotes, safety +/// checks), but is intended to be used in testing and simulation of the +/// testing and simulation ESP code. +/// +/// Notable absences in logic: Cooldown can be bypassed by first stopping. + +use anyhow::Result; +use log::*; //{trace, debug, info, warn, error} +use std::sync::Arc; + +use crate::fake_motor::{Motors,MotorStatus}; +use crate::fake_limits::{Limits, LimitStatus, EMPTY_LIMITS}; +use crate::fake_status::Statuses; +use crate::dispatch::{Dispatch, DispatchRecvQ, DispatchSendQ}; +use crate::commands::{Commands}; + +pub struct FakePic { + motor_state: Motors, + limit_state: Limits, + status_state: Statuses, + fault_cause: String, + send_q: DispatchSendQ, + recv_q: DispatchRecvQ, +} + +impl FakePic { + // Construct a FakePic with some default testing hardware + pub fn with_defaults(dp: &mut Dispatch) -> Self { + let cmd_filter = vec![ + Commands::FPicToggleAux, + Commands::FPicToggleAuto, + Commands::FPicPressLearn, + Commands::FPicPressUp, + Commands::FPicPressDown, + Commands::FPicPressStop, + Commands::FPicTogglePanic, + Commands::FPicLockout, + Commands::FPicFault{data: Arc::::new("".to_string())}, + Commands::FPicLimit{data: EMPTY_LIMITS}, + Commands::BluetoothAuto{data: crate::Button::Released}, + Commands::BluetoothAux{data: crate::Button::Released}, + Commands::BluetoothLearn{data: crate::Button::Released}, + Commands::BluetoothUp{data: crate::Button::Released}, + Commands::BluetoothDown{data: crate::Button::Released}, + Commands::BluetoothStop{data: crate::Button::Released}, + Commands::PairTimerExpired, + Commands::StopTimerExpired, + Commands::ButtonTimerExpired, + ]; + FakePic { + motor_state: Motors{ + // Two motors (initially stopped) + only_starboard_bow: MotorStatus::Stopped, + port_bow: MotorStatus::Stopped, + starboard_quarter: MotorStatus::NotPresent, + port_quarter: MotorStatus::NotPresent, + }, + limit_state: Limits{ + only_starboard_bow: LimitStatus::NotActive, + port_bow: LimitStatus::NotActive, + starboard_quarter: LimitStatus::NotPresent, + port_quarter: LimitStatus::NotPresent, + }, + status_state: Statuses::empty(), + fault_cause: "".to_string(), + send_q: dp.get_cmd_channel(), + recv_q: dp.get_callback_channel(&cmd_filter), + } + } + // === Change Fake Hardware Parameters ====================================================== + pub fn change_limits(&mut self, l: Limits) -> () { + self.limit_state = l; + } + pub fn change_motors(&mut self, m: Motors) -> () { + self.motor_state = m; + } + // === Handle button presses on the PIC ===================================================== + pub async fn toggle_aux(&mut self) -> Result<()> { + self.status_state ^= Statuses::AUX; + self.send_q.send(Commands::BluetoothStatusStatus { data: self.status_state }).await?; + Ok(()) + } + pub async fn toggle_auto(&mut self) -> Result<()> { + self.status_state ^= Statuses::AUTO; + self.send_q.send(Commands::BluetoothStatusStatus { data: self.status_state }).await?; + Ok(()) + } + pub async fn press_learn(&mut self) -> Result<()> { + self.status_state |= Statuses::LEARN; + self.send_q.send(Commands::AllowPairing).await?; //This starts the timer, and changes bluetooth modes + Ok(()) + } + pub async fn exit_learn(&mut self) -> Result<()> { + self.status_state -= Statuses::LEARN; + // Other changes, such as changing the bluetooth modes, are handled in the ble_server when it gets this event + Ok(()) + } + pub async fn press_up(&mut self) -> Result<()> { + if self.status_state.union(Statuses::PANIC).is_empty() || self.status_state.union(Statuses::LOCKOUT).is_empty(){ + warn!("Ignoring commands while in panic or lockout mode!") + } + if self.motor_state.are_cooldown() { + info!("Ignoring commands while motors cool down") + } + else if self.motor_state.going_down() { + warn!("Reversing from down to up; entering cooldown!"); + self.motor_state.update_status(MotorStatus::Cooldown); + self.send_q.send(Commands::StopTimerRestart).await?; + self.send_q.send(Commands::BluetoothStatusMotor { data: self.motor_state }).await?; + } + else { + info!("Starting motors moving up!"); + self.motor_state.update_status(MotorStatus::GoingUp); + self.send_q.send(Commands::ButtonTimerRestart).await?; + self.send_q.send(Commands::BluetoothStatusMotor{ data: self.motor_state }).await?; + } + Ok(()) + } + pub async fn press_down(&mut self) -> Result<()> { + if self.status_state.union(Statuses::PANIC).is_empty() || self.status_state.union(Statuses::LOCKOUT).is_empty() { + warn!("Ignoring commands while in panic or lockout mode!") + } + else if self.motor_state.are_cooldown() { + info!("Ignoring commands while motors cool down") + } + else if self.motor_state.going_up() { + warn!("Reversing from up to down; entering cooldown!"); + self.motor_state.update_status(MotorStatus::Cooldown); + self.send_q.send(Commands::StopTimerRestart).await?; + self.send_q.send(Commands::BluetoothStatusMotor { data: self.motor_state }).await?; + } + else { + info!("Starting motors moving down!"); + self.motor_state.update_status(MotorStatus::GoingDown); + self.send_q.send(Commands::ButtonTimerRestart).await?; + self.send_q.send(Commands::BluetoothStatusMotor{ data: self.motor_state }).await?; + } + Ok(()) + } + pub async fn press_stop(&mut self) -> Result<()> { + self.motor_state.stop(); + self.send_q.send(Commands::BluetoothStatusMotor { data: self.motor_state }).await?; + Ok(()) + } + pub async fn toggle_panic(&mut self) -> Result<()> { + self.status_state ^= Statuses::PANIC; + self.motor_state.stop(); + self.send_q.send(Commands::BluetoothStatusStatus { data: self.status_state }).await?; + self.send_q.send(Commands::BluetoothStatusMotor { data: self.motor_state }).await?; + Ok(()) + } + pub async fn lockout(&mut self) -> Result<()> { + self.status_state &= Statuses::LOCKOUT; // No toggle; lockout can only be cleared by reboot per spec + self.motor_state.stop(); + self.send_q.send(Commands::BluetoothStatusStatus { data: self.status_state }).await?; + self.send_q.send(Commands::BluetoothStatusMotor { data: self.motor_state }).await?; + Ok(()) + } + pub async fn fault(&mut self, cause: String) -> Result<()> { + if cause.is_empty() { + self.status_state -= Statuses::FAULT; + } else { + self.status_state |= Statuses::FAULT; + } + self.fault_cause = cause; + self.send_q.send(Commands::BluetoothStatusStatus { data: self.status_state }).await?; + Ok(()) + } + pub async fn change_limit(&mut self, limits: Limits) -> Result<()> { + self.limit_state = limits; + self.send_q.send(Commands::BluetoothStatusLimits {data: self.limit_state.clone() }).await?; + Ok(()) + } + pub async fn exit_cooldown(&mut self) -> Result<()> { + self.motor_state.finish_cooldown(); + self.send_q.send(Commands::BluetoothStatusMotor { data: self.motor_state }).await?; + Ok(()) + } + + // === Handle events from event manager ===================================================== + pub async fn run(&mut self) -> Result<()> { + let cmd = self.recv_q.recv().await.expect("PIC simulator failed waiting for messages"); + match cmd { + // Commands from testing and user + Commands::FPicToggleAux => {self.toggle_aux().await} + Commands::FPicToggleAuto => {self.toggle_auto().await} + Commands::FPicPressLearn => {self.press_learn().await} + Commands::FPicPressUp => {self.press_up().await} + Commands::FPicPressDown => {self.press_down().await} + Commands::FPicPressStop => {self.press_stop().await} + Commands::FPicTogglePanic => {self.toggle_panic().await} + Commands::FPicLockout => {self.lockout().await} + Commands::FPicFault{data} => {self.fault(data.to_string()).await} + Commands::FPicLimit { data } => {self.change_limit(data).await} + // Commands from bluetooth + Commands::BluetoothAuto { data } => { + if data.is_pressed() { + self.toggle_auto().await?; + } Ok(()) + } + Commands::BluetoothAux { data } => { + if data.is_pressed() { + self.toggle_aux().await?; + } Ok(()) + } + Commands::BluetoothLearn { data } => { + if data.is_pressed() { + self.press_learn().await?; + } Ok(()) + } + Commands::BluetoothUp { data } => { + if data.is_pressed() { + self.press_up().await?; + } else { + self.press_stop().await?; // Releasing up is equivalent to stop + } Ok(()) + } + Commands::BluetoothDown { data } => { + if data.is_pressed() { + self.press_down().await?; + } else { + self.press_stop().await?; // Releasing down is equivalent to stop + } Ok(()) + } + Commands::BluetoothStop { .. } => {self.press_stop().await} // Stopping on release of stop button is a noop but safe. + // Commands from timers + Commands::PairTimerExpired => {self.exit_learn().await} + Commands::StopTimerExpired => {self.exit_cooldown().await} + Commands::ButtonTimerExpired => {self.press_stop().await} + _ => {warn!("Unknown command received by Fake PIC simulator!"); Ok(())} + } + } + +} \ No newline at end of file diff --git a/gem-remotes-lib/src/fake_status.rs b/gem-remotes-lib/src/fake_status.rs new file mode 100644 index 0000000..32baca7 --- /dev/null +++ b/gem-remotes-lib/src/fake_status.rs @@ -0,0 +1,28 @@ +/// Fake device status tracking the various statuses the PIC can have. + +use bitflags::bitflags; + +bitflags! { + #[derive(Debug, Clone, Copy)] + pub struct Statuses: u16 { + const LOCKOUT = 1 << 0; + const PANIC = 1 << 1; + const FAULT = 1 << 2; + const LEARN = 1 << 3; + const AUTO = 1 << 4; + const AUX = 1 << 5; + } +} + +impl Statuses { + pub fn to_le_bytes(&self) -> [u8;2] { + self.bits().to_le_bytes() + } + pub fn from_le_bytes(b: &[u8]) -> Statuses { + // No sanity checking is done; but this function is not used even in test code right now. + let mut bytes = [0;2]; + bytes[0] = *b.get(0).unwrap(); + bytes[1] = *b.get(1).unwrap(); + Statuses::from_bits(u16::from_le_bytes(bytes)).unwrap() + } +} \ No newline at end of file diff --git a/gem-remotes-lib/src/lib.rs b/gem-remotes-lib/src/lib.rs index 32c8cad..b89eb81 100644 --- a/gem-remotes-lib/src/lib.rs +++ b/gem-remotes-lib/src/lib.rs @@ -4,26 +4,36 @@ // Modules in this crate pub mod commands; pub mod dispatch; -pub mod motor_controller; +pub mod fake_pic; +pub mod fake_limits; +pub mod fake_motor; +pub mod fake_status; +//pub mod ble_lift_service; // Re-published items pub use commands::{ Button, - Commands -}; -pub use motor_controller::{ - AutoMode, - Controller, - LimitState, - MotorCommands, - MotorRecvQ, - MotorSendQ, + Commands, }; pub use dispatch::{ Dispatch, DispatchSendQ, DispatchRecvQ, }; +pub use fake_limits::{ + Limits, + LimitStatus, + EMPTY_LIMITS +}; +pub use fake_motor::{ + Motors, + MotorStatus, + EMPTY_MOTORS, +}; +pub use fake_status::Statuses; +pub use fake_pic::FakePic; +//pub use ble_lift_service::BleLiftService; + diff --git a/gem-remotes-lib/src/motor_controller.rs b/gem-remotes-lib/src/motor_controller.rs deleted file mode 100644 index 1ecd1cd..0000000 --- a/gem-remotes-lib/src/motor_controller.rs +++ /dev/null @@ -1,766 +0,0 @@ -/// State machine of the motor's current state, that takes as inputs -/// command messages. - -use log::*; //{trace, debug, info, warn, error} -use anyhow::{anyhow, Result}; -use async_channel::{Receiver, Sender}; -use crate::commands::{Commands, Button, Toggle}; -use crate::dispatch::{Dispatch, DispatchRecvQ, DispatchSendQ}; - -// The main internal state of the controller, representing the current control method of the motors. -#[derive(Clone, Copy, Debug, PartialEq)] -enum ControllerStates { - Stopped, - Stopping, - GoingUp, - AutoUp, - GoingDown, - AutoDown, -} - -#[derive(Clone, Copy, Debug, PartialEq)] -pub enum MotorCommands { - StartUp, - StartDown, - Stop -} - -pub type MotorSendQ = Sender; -pub type MotorRecvQ = Receiver; - -#[derive(Clone, Copy, Debug, PartialEq)] -pub enum AutoMode { - Disallowed = 0, - Allowed = 1, -} - -#[derive(Clone, Copy, Debug, PartialEq)] -pub enum LimitState { - NoLimitsHit, - UpperHit, - LowerHit, - BothHit, -} - -pub struct Controller { - state: ControllerStates, - recv: DispatchRecvQ, - send: DispatchSendQ, - motor_q: MotorSendQ, - auto_mode: AutoMode, - limit_state: LimitState, -} - -impl Controller { - pub fn new(recv: DispatchRecvQ, send: DispatchSendQ, motor_q: MotorSendQ) -> Self { - Controller { - state: ControllerStates::Stopping, - recv: recv, - send: send, - motor_q: motor_q, - auto_mode: AutoMode::Disallowed, // Use safe default - limit_state: LimitState::BothHit, // Use safe default - } - } - - /// Tell the message dispatch which messages we are interested in receiving, and get - /// a callback channel that receives those messages. - pub fn prepare_controller(dp: &mut Dispatch) -> DispatchRecvQ { - let cmds = vec![ - Commands::PicRecvUp{data: Button::Released}, - Commands::PicRecvDown{data: Button::Released}, - Commands::PicRecvStop{data: Button::Released}, - Commands::BluetoothUp{data: Button::Released}, - Commands::BluetoothDown{data: Button::Released}, - Commands::BluetoothStop{data: Button::Released}, - Commands::PicRecvLimitUp{data: Button::Released}, - Commands::PicRecvLimitDown{data: Button::Released}, - Commands::PicRecvAutoMode{data: Toggle::Inactive}, - Commands::StopTimerExpired, - Commands::ButtonTimerExpired, - ]; - dp.get_callback_channel(&cmds) - } - - async fn enter_state(&mut self, new_s: &ControllerStates) -> Result<()> { - match new_s { - ControllerStates::Stopped => { - // Other notify commands are sent directly from the motor controller - self.send.send(Commands::NotifyMotorStop{data: Button::Released}).await?; - } - ControllerStates::Stopping => { - self.send.send(Commands::StopTimerRestart).await?; - self.motor_q.send(MotorCommands::Stop).await?; - self.send.send(Commands::NotifyMotorStop{data: Button::Pressed}).await?; - } - ControllerStates::GoingUp => { - self.send.send(Commands::ButtonTimerRestart).await?; - self.motor_q.send(MotorCommands::StartUp).await?; - self.send.send(Commands::NotifyMotorUp{data: Button::Pressed}).await?; - } - ControllerStates::AutoUp => { - self.motor_q.send(MotorCommands::StartUp).await?; - self.send.send(Commands::NotifyMotorUp{data: Button::Pressed}).await?; - } - ControllerStates::GoingDown => { - self.send.send(Commands::ButtonTimerRestart).await?; - self.motor_q.send(MotorCommands::StartDown).await?; - self.send.send(Commands::NotifyMotorUp{data: Button::Pressed}).await?; - } - ControllerStates::AutoDown => { - self.motor_q.send(MotorCommands::StartDown).await?; - self.send.send(Commands::NotifyMotorUp{data: Button::Pressed}).await?; - } - } - Ok(()) - } - - async fn exit_state(&mut self, old_s: &ControllerStates) -> Result <()> { - match old_s { - ControllerStates::Stopped => {} - ControllerStates::Stopping => { - self.send.send(Commands::StopTimerClear).await?; - } - ControllerStates::GoingUp => { - self.send.send(Commands::ButtonTimerClear).await?; - self.send.send(Commands::NotifyMotorUp{data: Button::Released}).await?; - } - ControllerStates::AutoUp => { - self.send.send(Commands::NotifyMotorUp{data: Button::Released}).await?; - } - ControllerStates::GoingDown => { - self.send.send(Commands::ButtonTimerClear).await?; - self.send.send(Commands::NotifyMotorDown{data: Button::Released}).await?; - } - ControllerStates::AutoDown => { - self.send.send(Commands::NotifyMotorDown{data: Button::Released}).await?; - } - } - Ok(()) - } - - fn change_state_if_released(&self, data: &Button, new_state: ControllerStates) -> ControllerStates { - match data { - Button::Released => {new_state} - Button::Pressed => {self.state.clone()} - } - } - - fn change_state_if_pressed(&self, data: &Button, new_state: ControllerStates) -> ControllerStates { - match data { - Button::Released => {self.state.clone()} - Button::Pressed => {new_state} - } - } - - /// Determines the state the controller should be in based on the command received. - async fn handle_cmd(&mut self, cmd: &Commands) -> Result { - let mut rc = self.state.clone(); // Don't transition by default. - match cmd { - Commands::PicRecvUp { data } | Commands::BluetoothUp { data }=> { - match self.state { - ControllerStates::Stopped => {rc = self.change_state_if_pressed(data, self.remote_up_or_auto_up())} - ControllerStates::Stopping => {} - ControllerStates::GoingUp => { - self.send.send(Commands::ButtonTimerRestart).await.expect("Failed to necessary timer"); - rc = self.change_state_if_released(data, ControllerStates::Stopping) - } - ControllerStates::AutoUp => {} // Don't stop auto on button release - ControllerStates::GoingDown | - ControllerStates::AutoDown => { - rc= self.change_state_if_pressed(data, ControllerStates::Stopping) - } - } - } - Commands::PicRecvDown { data } | Commands::BluetoothDown { data } => { - match self.state { - ControllerStates::Stopped => {rc = self.change_state_if_pressed(data, self.remote_down_or_auto_down())} - ControllerStates::Stopping => {} - ControllerStates::GoingUp | - ControllerStates::AutoUp => { - rc = self.change_state_if_pressed(data, ControllerStates::Stopping) - } - ControllerStates::GoingDown => { - self.send.send(Commands::ButtonTimerRestart).await.expect("Failed to necessary timer"); - rc = self.change_state_if_released(data, ControllerStates::Stopping) - } - ControllerStates::AutoDown => {} - } - } - Commands::PicRecvStop { data } | Commands::BluetoothStop { data } => { - match self.state { - ControllerStates::Stopped => {} - ControllerStates::Stopping => {} - ControllerStates::GoingUp | - ControllerStates::AutoUp | - ControllerStates::GoingDown | - ControllerStates::AutoDown => { - rc = self.change_state_if_pressed(data, ControllerStates::Stopping) - } - } - } - Commands::PicRecvLimitUp { data } => { - self.adjust_limit(LimitState::UpperHit, data); - match self.state { - ControllerStates::Stopped => {} // Ignore; this could just be our initial notification on startup. - ControllerStates::Stopping => {} - ControllerStates::GoingUp | - ControllerStates::AutoUp=> { - released_warning(data, "Limit switches may be installed incorrectly!"); - rc = self.change_state_if_pressed(data, ControllerStates::Stopping) - } - ControllerStates::GoingDown | - ControllerStates::AutoDown=> { - pressed_warning(data, "Limit switches may be installed incorrectly!"); - // Stop out of an abundance of caution. We should not get a limit press, even if it's the wrong one. - rc = self.change_state_if_pressed(data, ControllerStates::Stopping) - } - } - } - Commands::PicRecvLimitDown { data } => { - self.adjust_limit(LimitState::LowerHit, data); - match self.state { - ControllerStates::Stopped => {} // Ignore; this could just be our initial notification on startup. - ControllerStates::Stopping => {} - ControllerStates::GoingUp | - ControllerStates::AutoUp => { - pressed_warning(data, "Limit switches may be installed incorrectly!"); - // Stop out of an abundance of caution. We should not get a limit press, even if it's the wrong one. - rc = self.change_state_if_pressed(data, ControllerStates::Stopping) - } - ControllerStates::GoingDown | - ControllerStates::AutoDown => { - released_warning(data, "Limit switches may be installed incorrectly!"); - rc = self.change_state_if_pressed(data, ControllerStates::Stopping) - } - } - } - Commands::PicRecvAutoMode { data } => { - self.set_auto(data); - match self.state { - ControllerStates::Stopped => {} - ControllerStates::Stopping => {} - ControllerStates::GoingUp => {} - ControllerStates::AutoUp => {rc = ControllerStates::GoingUp} - ControllerStates::GoingDown => {} - ControllerStates::AutoDown => {rc = ControllerStates::GoingDown} - } - } - Commands::StopTimerExpired => { - match self.state { - ControllerStates::Stopped => {} - ControllerStates::Stopping => {rc = ControllerStates::Stopped} - ControllerStates::GoingUp | - ControllerStates::AutoUp | - ControllerStates::GoingDown | - ControllerStates::AutoDown => { - warn!("Stop timer returned in an inappropriate state! {:?}", self.state) - } - } - } - Commands::ButtonTimerExpired => { - match self.state { - ControllerStates::Stopped => {} - ControllerStates::Stopping => {} - ControllerStates::GoingUp => {rc = ControllerStates::Stopping} - ControllerStates::AutoUp => {} - ControllerStates::GoingDown => {rc = ControllerStates::Stopping} - ControllerStates::AutoDown => {} - } - } - // Commands we should ignore (not using _ because we want to ensure all commands are accounted for!) - Commands::StopTimerRestart | - Commands::StopTimerClear | - Commands::ButtonTimerRestart | - Commands::ButtonTimerClear | - Commands::PairTimerClear | - Commands::PairTimerExpired | - Commands::AllowPairing | - Commands::EraseBleBonds => { - warn!("Unexpected command received by motor controller") // TODO: internal "us" error. - } - Commands::NotifyMotorDown { data } | - Commands::NotifyMotorStop { data } | - Commands::NotifyMotorUp { data } => { - warn!("Unexpected command received by motor controller {:?}", data) // TODO: internal "us" error. - } - Commands::BluetoothName { data } => { - warn!("Unexpected command received by motor controller {:?}", data) // TODO: internal "us" error. - } - Commands::TestingExit => { - return Err(anyhow!("Exiting due to testing")) - } - } - //self.state.clone() // Don't transition by default - Ok(rc) - } - - async fn transition_state(&mut self, old_s: &ControllerStates, new_s: &ControllerStates) -> Result<()> { - if old_s != new_s { - self.exit_state(&old_s).await?; - self.enter_state(&new_s).await?; - } - self.state = new_s.clone(); - Ok(()) - } - - pub async fn run(&mut self) -> Result<()> { - // On entry, assume initial stopping state - debug!("Setting motor controller initial state to stopping"); - self.state = ControllerStates::Stopping; - self.enter_state(&ControllerStates::Stopping).await?; - loop { - let cmd = self.recv.recv().await.expect("Motor controller command queue unexpectedly failed"); - trace!("Got command {:?}",cmd); - match self.handle_cmd(&cmd).await { - Ok(new_s) => { - trace!("State current {:?} new {:?}", self.state, new_s); - self.transition_state(&self.state.clone(), &new_s) - .await - .expect("Unexpected state change failure in motor controller"); - } - Err(_) => {break;} - } - } - Err(anyhow!("Unexpectedly exited loop")) - } - - fn remote_up_or_auto_up(&self) -> ControllerStates { - self.up_or_auto_up(ControllerStates::GoingUp) - } - - fn up_or_auto_up(&self, up: ControllerStates) -> ControllerStates { - // Assume that checking the limit against the direction has already been performed. - match self.auto_mode { - AutoMode::Disallowed => { - up // TODO: this allows manual buttons to override limits as long as "auto" mode is off. - } - AutoMode::Allowed => { - match self.limit_state { - LimitState::NoLimitsHit | LimitState::LowerHit=> { - ControllerStates::AutoUp - } - LimitState::UpperHit => { - ControllerStates::Stopping // Failsafe as we are already at our upper limit. TODO: maybe should be manual up? - } - LimitState::BothHit => { - up - } - } - } - } - } - - fn remote_down_or_auto_down(&self) -> ControllerStates { - self.down_or_auto_down(ControllerStates::GoingDown) - } - - fn down_or_auto_down(&self, down: ControllerStates) -> ControllerStates { - // Assume that checking the limit against the direction has already been performed. - match self.auto_mode { - AutoMode::Disallowed => { - down // TODO: this allows manual buttons to override limits as long as "auto" mode is off. - } - AutoMode::Allowed => { - match self.limit_state { - LimitState::NoLimitsHit | LimitState::LowerHit=> { - ControllerStates::AutoDown - } - LimitState::UpperHit => { - ControllerStates::Stopping // Failsafe as we are already at our upper limit. TODO: Maybe should be manual down? - } - LimitState::BothHit => { - down - } - } - } - } - } - - fn set_auto(&mut self, data: &Toggle) { - match data { - Toggle::Inactive => { - self.auto_mode = AutoMode::Disallowed; - } - Toggle::Active => { - if self.limit_state == LimitState::BothHit { - warn!("Limit switches not detected. Aborting auto mode."); - } else { - self.auto_mode = AutoMode::Allowed; - } - } - } - } - - // Adjusts the current limit state, based on its present state and an incoming button press or release. - fn adjust_limit(&mut self, limit: LimitState, pressed: &Button) { - match pressed { - Button::Released => { - match limit { - LimitState::NoLimitsHit => { - unreachable!("There is no way to press NoLimits") - } - LimitState::LowerHit => { - match self.limit_state { - LimitState::NoLimitsHit | - LimitState::UpperHit => {warn!("removed limit we never hit {:?}", LimitState::LowerHit);} //TODO intenral error - LimitState::LowerHit => {self.limit_state = LimitState::NoLimitsHit;} - LimitState::BothHit => {self.limit_state = LimitState::UpperHit;} - } - } - LimitState::UpperHit => { - match self.limit_state { - LimitState::NoLimitsHit | - LimitState::LowerHit => {warn!("removed limit we never hit {:?}", LimitState::UpperHit);} //TODO intenral error - LimitState::UpperHit => {self.limit_state = LimitState::NoLimitsHit;} - LimitState::BothHit => {self.limit_state = LimitState::LowerHit;} - } - } - LimitState::BothHit => { - unreachable!("There is no way to press BothHit") - } - } - } - Button::Pressed => { - match limit { - LimitState::NoLimitsHit => { - unreachable!("There is no way to press BothHit") - } - LimitState::LowerHit => { - match self.limit_state { - LimitState::NoLimitsHit => {self.limit_state = LimitState::LowerHit;} - LimitState::LowerHit => {} - LimitState::UpperHit => {self.limit_state = LimitState::BothHit;} - LimitState::BothHit => {} - } - } - LimitState::UpperHit => { - match self.limit_state { - LimitState::NoLimitsHit => {self.limit_state = LimitState::UpperHit;} - LimitState::LowerHit => {self.limit_state = LimitState::BothHit;} - LimitState::UpperHit => {} - LimitState::BothHit => {} - } - } - LimitState::BothHit => { - unreachable!("There is no way to press BothHit") - } - } - } - } - } - -} - -// Give a user warning if the given button state is pressed at the time -fn pressed_warning(data: &Button, warn: &str) { - match data { - Button::Pressed => {warn!("{}", warn);} // TODO: user warning, not internal - Button::Released => {} - } -} - -// Give a user warning if the given button state is released at the time -fn released_warning(data: &Button, warn: &str) { - match data { - Button::Released => {warn!("{}", warn);} // TODO: user warning, not internal - Button::Pressed => {} - } -} - -//// Test //////////////////////////////////////////////////////////////////////////////////////// - -#[cfg(test)] -mod tests { - use super::*; - use async_channel::{unbounded, TryRecvError}; - use async_io::block_on; - - // Creates a new controller for use in tests. Has async-channen send, recv, and motor - // endpoints to use. Starts in AutoMode::Disallowed and LimitState::BothHit - fn create_controller() -> (Controller, DispatchSendQ, DispatchRecvQ, MotorRecvQ) { - let (ch1s, ch1r) = unbounded(); - let (ch2s, ch2r) = unbounded(); - let (chms, chmr) = unbounded(); - let con = Controller::new(ch1r, ch2s, chms); - (con, ch1s, ch2r, chmr) - } - - // Send a series of messages to the controller, then exit. - fn run_cmd_queue(con: &mut Controller, cs: DispatchSendQ, q: &mut Vec) -> Result<()>{ - block_on ( - async { - q.push(Commands::TestingExit); - for c in q { - cs.send(c.clone()).await? - } - let _ = con.run().await; // If this doesn't error out of the loop, we'll hang here - Ok(()) - } - ) - } - - /* States we want to test: - Controller states: - Stopped, - Stopping, - GoingUp, - AutoUp, - GoingDown, - AutoDown, - Auto states: - Disallowed - Allowed - Limit states: - NoLimitsHit, - UpperHit, - LowerHit, - BothHit, - */ - - #[test] - fn motor_stops_on_initialization() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::Stopping); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - - #[test] - fn motor_stop_state_after_timeout() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::Stopped); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - - #[test] - fn motor_refuses_auto_when_bothhit() -> Result<()>{ - let (mut con, cs, _cr, _mr) = create_controller(); - let mut q = vec![ - Commands::PicRecvAutoMode{data: Toggle::Active}, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.auto_mode, AutoMode::Disallowed); - Ok(()) - } - - #[test] - fn motor_succeeds_auto_when_not_bothhit() -> Result<()>{ - let (mut con, cs, _cr, _mr) = create_controller(); - let mut q = vec![ - Commands::PicRecvLimitDown { data: Button::Released }, - Commands::PicRecvAutoMode{data: Toggle::Active}, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.auto_mode, AutoMode::Allowed); - Ok(()) - } - - #[test] - fn motor_enters_timed_up() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - Commands::PicRecvUp { data: Button::Pressed } - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::GoingUp); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - - #[test] - fn motor_stops_after_no_input_no_auto() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - Commands::BluetoothDown { data: Button::Pressed }, - Commands::ButtonTimerExpired, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::Stopping); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartDown); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - - #[test] - fn motor_stops_on_release_timed_up() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - Commands::PicRecvUp { data: Button::Pressed }, - Commands::PicRecvUp { data: Button::Released }, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::Stopping); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - #[test] - fn motor_stops_on_limit_timed_up() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - Commands::PicRecvUp { data: Button::Pressed }, - Commands::PicRecvLimitUp { data: Button::Pressed }, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::Stopping); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - - #[test] - fn motor_enters_auto_up() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - Commands::PicRecvLimitUp{data: Button::Released}, - Commands::PicRecvAutoMode{data: Toggle::Active}, - Commands::PicRecvUp { data: Button::Pressed }, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::AutoUp); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - - #[test] - fn motor_does_not_stop_after_no_input_in_auto() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - Commands::PicRecvLimitUp{data: Button::Released}, - Commands::PicRecvAutoMode{data: Toggle::Active}, - Commands::BluetoothDown { data: Button::Pressed }, - Commands::ButtonTimerExpired, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::AutoDown); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartDown); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - - #[test] - fn motor_does_not_stop_auto_release() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - Commands::PicRecvLimitUp{data: Button::Released}, - Commands::PicRecvAutoMode{data: Toggle::Active}, - Commands::PicRecvUp { data: Button::Pressed }, - Commands::PicRecvUp { data: Button::Released }, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::AutoUp); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - - #[test] - fn motor_switches_to_timed_when_auto_toggled_off() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - Commands::PicRecvLimitUp{data: Button::Released}, - Commands::PicRecvAutoMode{data: Toggle::Active}, - Commands::PicRecvUp { data: Button::Pressed }, - Commands::PicRecvAutoMode{data: Toggle::Inactive}, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::GoingUp); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp); // Second up commanded should be okay - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - - #[test] - fn motor_continues_with_additional_presses() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - Commands::PicRecvLimitUp{data: Button::Released}, - Commands::PicRecvAutoMode{data: Toggle::Active}, - Commands::PicRecvUp { data: Button::Pressed }, - Commands::PicRecvUp { data: Button::Released }, - Commands::BluetoothUp { data: Button::Pressed }, - Commands::BluetoothUp { data: Button::Released }, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::AutoUp); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - - #[test] - fn motor_stops_on_opposite_button() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - Commands::PicRecvLimitUp{data: Button::Released}, - Commands::PicRecvAutoMode{data: Toggle::Active}, - Commands::PicRecvUp { data: Button::Pressed }, - Commands::PicRecvUp { data: Button::Released }, - Commands::BluetoothDown { data: Button::Pressed }, - Commands::BluetoothDown { data: Button::Released }, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::Stopping); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } - - #[test] - fn motor_stops_if_limits_installed_backwards() -> Result<()>{ - let (mut con, cs, _cr, mr) = create_controller(); - let mut q = vec![ - Commands::StopTimerExpired, - Commands::PicRecvLimitUp{data: Button::Released}, - Commands::PicRecvLimitDown{data: Button::Released}, - Commands::PicRecvAutoMode{data: Toggle::Active}, - Commands::PicRecvUp { data: Button::Pressed }, - Commands::PicRecvUp { data: Button::Released }, - Commands::PicRecvLimitDown { data: Button::Pressed }, - ]; - run_cmd_queue(&mut con, cs, &mut q)?; - assert_eq!(con.state, ControllerStates::Stopping); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::StartUp); - assert_eq!(mr.try_recv().unwrap(), MotorCommands::Stop); - assert_eq!(mr.try_recv(), Err(TryRecvError::Empty)); // Make sure queue is empty - Ok(()) - } -}