split into hardware dependent / independent
This commit is contained in:
445
gem-remotes-lib/src/motor_controller.rs
Normal file
445
gem-remotes-lib/src/motor_controller.rs
Normal file
@ -0,0 +1,445 @@
|
||||
/// State machine of the motor's current state, that takes as inputs
|
||||
/// command messages.
|
||||
|
||||
use log::*; //{trace, debug, info, warn, error}
|
||||
use anyhow::Result;
|
||||
use async_channel::{Receiver, Sender};
|
||||
use crate::commands::{Commands, Button};
|
||||
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)]
|
||||
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: Button::Released},
|
||||
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) -> ControllerStates {
|
||||
match cmd {
|
||||
Commands::PicRecvUp { data } | Commands::BluetoothUp { data }=> {
|
||||
match self.state {
|
||||
ControllerStates::Stopped => {return 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");
|
||||
return self.change_state_if_released(data, ControllerStates::Stopping)
|
||||
}
|
||||
ControllerStates::AutoUp => {} // Don't stop auto on button release
|
||||
ControllerStates::GoingDown |
|
||||
ControllerStates::AutoDown => {
|
||||
return self.change_state_if_pressed(data, ControllerStates::Stopping)
|
||||
}
|
||||
}
|
||||
}
|
||||
Commands::PicRecvDown { data } | Commands::BluetoothDown { data } => {
|
||||
match self.state {
|
||||
ControllerStates::Stopped => {return self.change_state_if_pressed(data, self.remote_down_or_auto_down())}
|
||||
ControllerStates::Stopping => {}
|
||||
ControllerStates::GoingUp |
|
||||
ControllerStates::AutoUp => {
|
||||
return self.change_state_if_pressed(data, ControllerStates::Stopping)
|
||||
}
|
||||
ControllerStates::GoingDown => {
|
||||
self.send.send(Commands::ButtonTimerRestart).await.expect("Failed to necessary timer");
|
||||
return 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 => {
|
||||
return 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!");
|
||||
return 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.
|
||||
return 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.
|
||||
return self.change_state_if_pressed(data, ControllerStates::Stopping)
|
||||
}
|
||||
ControllerStates::GoingDown |
|
||||
ControllerStates::AutoDown => {
|
||||
released_warning(data, "Limit switches may be installed incorrectly!");
|
||||
return self.change_state_if_pressed(data, ControllerStates::Stopping)
|
||||
}
|
||||
}
|
||||
}
|
||||
Commands::PicRecvAutoMode { data } => {self.set_auto(data);}
|
||||
Commands::StopTimerExpired => {
|
||||
match self.state {
|
||||
ControllerStates::Stopped => {}
|
||||
ControllerStates::Stopping => {return 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 => {return ControllerStates::Stopping}
|
||||
ControllerStates::AutoUp => {}
|
||||
ControllerStates::GoingDown => {return 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.
|
||||
}
|
||||
}
|
||||
self.state.clone() // Don't transition by default
|
||||
}
|
||||
|
||||
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);
|
||||
let new_s = self.handle_cmd(&cmd).await;
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
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: &Button) {
|
||||
match data {
|
||||
Button::Released => {
|
||||
self.auto_mode = AutoMode::Disallowed;
|
||||
}
|
||||
Button::Pressed => {
|
||||
if self.limit_state == LimitState::BothHit {
|
||||
warn!("Limit switches not detected. Aborting auto mode.");
|
||||
} else {
|
||||
self.auto_mode = AutoMode::Allowed;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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")
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
fn pressed_warning(data: &Button, warn: &str) {
|
||||
match data {
|
||||
Button::Pressed => {warn!("{}", warn);} // TODO: user warning, not intenral
|
||||
Button::Released => {}
|
||||
}
|
||||
}
|
||||
|
||||
fn released_warning(data: &Button, warn: &str) {
|
||||
match data {
|
||||
Button::Released => {warn!("{}", warn);} // TODO: user warning, not intenral
|
||||
Button::Pressed => {}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user