GEM-Remotes/GemBluetoothEmbeddedDevice
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial restructuring after _actual_ spec added

Browse Source
This commit is contained in:
Dave
2024-11-03 12:52:18 -05:00
parent 84a105639c
commit 045237d44f
15 changed files with 659 additions and 983 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
gem-remotes-esp32.code-workspace Normal file
View File

@ -0,0 +1,11 @@
{
"folders": [
{
"path": "gem-remotes-esp32"
},
{
"path": "gem-remotes-lib"
}
],
"settings": {}
}

140
gem-remotes-esp32/src/ble_server.rs
View File

@ -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::<String>::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.

15
gem-remotes-esp32/src/main.rs
View File

@ -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::<Commands, Commands>::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.

2
gem-remotes-esp32/src/message_timer.rs
View File

@ -30,6 +30,7 @@ pub struct MessageTimer <Q, S> {
}
impl<Q: PartialEq, S: Clone> MessageTimer<Q, S> {
/*
pub fn new(
restart: Q,
cancel: Q,
@ -48,6 +49,7 @@ impl<Q: PartialEq, S: Clone> MessageTimer<Q, S> {
state: State::Stopped,
}
}
*/
pub fn new_on_dispatch(
restart: Commands,

70
gem-remotes-esp32/src/motor_driver.rs
View File

@ -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.

34
gem-remotes-esp32/src/pair_button_driver.rs
View File

@ -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<Commands>;
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
}
}
}

54
gem-remotes-esp32/src/test_console.rs
View File

@ -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 {

1
gem-remotes-lib/Cargo.toml
View File

@ -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"

55
gem-remotes-lib/src/commands.rs
View File

@ -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>}, // 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<String>},
//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<String>},
// 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)
}
}
}

62
gem-remotes-lib/src/fake_limits.rs Normal file
View File

@ -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,
};

137
gem-remotes-lib/src/fake_motor.rs Normal file
View File

@ -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,
};

237
gem-remotes-lib/src/fake_pic.rs Normal file
View File

@ -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::<String>::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(())}
}
}
}

28
gem-remotes-lib/src/fake_status.rs Normal file
View File

@ -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()
}
}

30
gem-remotes-lib/src/lib.rs
View File

@ -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;

766
gem-remotes-lib/src/motor_controller.rs
View File

@ -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<MotorCommands>;
pub type MotorRecvQ = Receiver<MotorCommands>;
#[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<ControllerStates> {
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<Commands>) -> 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(())
}
}