Initial rough-in of motor controller

gem-remotes-esp32/src/motor_controller.rs

@@ -0,0 +1,192 @@
+/// State machine of the motor's current state, that takes as inputs
+/// command messages.
+
+use log::*; //{trace, debug, info, warn, error}
+//use async_channel::Receiver;
+use anyhow::Result;
+use std::boxed::Box;
+
+use crate::commands::Commands;
+use crate::motor_driver::MotorDriver;
+use crate::dispatch::{Dispatch, RecvQ, SendQ};
+
+#[derive(Debug, PartialEq, Clone)]
+enum ControllerStates {
+    Stopped,
+    Stopping,
+    GoingUp,
+    GoingDown,
+    //TODO: AutoUp and AutoDown
+}
+
+pub struct Controller {
+    state: ControllerStates,
+    recv: RecvQ,
+    send: SendQ,
+    driver: Box<dyn MotorDriver>,
+}
+
+impl Controller {
+    pub fn new(recv: RecvQ, send: SendQ, driver: Box<dyn MotorDriver>) -> Controller {
+        Controller {
+            state: ControllerStates::Stopping,
+            recv: recv,
+            send: send,
+            driver: driver}
+    }
+
+    /// 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) -> RecvQ {
+        let cmds = vec![
+            Commands::PicRecvUp,
+            Commands::PicRecvDown,
+            Commands::PicRecvStop,
+            Commands::BluetoothUp{data: 0},
+            Commands::BluetoothDown{data: 0},
+            Commands::BluetoothStop{_data: 0},
+            Commands::StopTimerExpired,
+            Commands::ButtonTimerExpired,
+        ];
+        dp.get_callback_channel(&cmds)
+    }
+
+    async fn enter_state(&mut self, new_s: &ControllerStates) -> Result<()> {
+        match new_s {
+            ControllerStates::Stopped => {}
+            ControllerStates::Stopping => {
+                self.send.send(Commands::StopTimerRestart).await?;
+                self.driver.stop()?;
+            }
+            ControllerStates::GoingUp => {
+                self.send.send(Commands::ButtonTimerRestart).await?;
+                self.driver.start_up()?;
+            }
+            ControllerStates::GoingDown => {
+                self.send.send(Commands::ButtonTimerRestart).await?;
+                self.driver.start_down()?;
+            }
+        }
+        Ok(())
+    }
+
+    /// Maintain the current state - e.g. reset button timers, etc.
+    async fn maintain_state(&mut self) -> Result<()> {
+        match self.state {
+            ControllerStates::Stopped => {}
+            ControllerStates::Stopping => {} // Do not reset this timer; let it expire.
+            ControllerStates::GoingUp => {self.send.send(Commands::ButtonTimerRestart).await?;}
+            ControllerStates::GoingDown => {self.send.send(Commands::ButtonTimerRestart).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?;
+            }
+            ControllerStates::GoingDown => {
+                self.send.send(Commands::ButtonTimerClear).await?;
+            }
+        }
+        Ok(())
+    }
+
+    /// Determines the state the controller should be in based on the command received.
+    fn handle_cmd(&mut self, cmd: &Commands) -> ControllerStates {
+        match self.state {
+
+            ControllerStates::Stopped => {
+                match cmd {
+                    Commands::PicRecvUp => {return ControllerStates::GoingUp}
+                    Commands::PicRecvDown => {return ControllerStates::GoingDown}
+                    Commands::BluetoothUp { data } => {
+                        match data {
+                            0 => {}
+                            1 => {return ControllerStates::GoingUp}
+                            _ => {error!("Invalid data sent via BluetoothUp: {}", data); return ControllerStates::Stopping}
+                        }
+                    }
+                    Commands::BluetoothDown { data } => {
+                        match data {
+                            0 => {}
+                            1 => {return ControllerStates::GoingDown}
+                            _ => {error!("Invalid data sent via BluetoothDown: {}", data); return ControllerStates::Stopping}
+                        }
+                    }
+                    _ => {} // Ignore other commands when stopped.
+                }
+            }
+
+            ControllerStates::Stopping => {
+                match cmd {
+                    Commands::StopTimerExpired => {return ControllerStates::Stopped}
+                    _ => {} // Ignore other commands when stopping
+                }
+            }
+
+            ControllerStates::GoingUp => {
+                match cmd {
+                    Commands::PicRecvUp => {return self.state.clone()}
+                    Commands::BluetoothUp { data } => {
+                        match data {
+                            0 => {} // Not an error, but transition to stop
+                            1 => {return self.state.clone()}
+                            _ => {error!("Invalid data sent via BluetoothUp: {}", data)}
+                        }
+                    }
+                    _ => {} // fall through to GoingUp default of stopping
+                }
+                return ControllerStates::Stopping; // Almost every command will be interpreted as stop
+            }
+
+            ControllerStates::GoingDown => {
+                match cmd {
+                    Commands::PicRecvDown => {return self.state.clone()}
+                    Commands::BluetoothDown { data } => {
+                        match data {
+                            0 => {} // Not an error, but transition to stop
+                            1 => {return self.state.clone()}
+                            _ => {error!("Invalid data sent via BluetoothDown: {}", data)}
+                        }
+                    }
+                    _ => {} // fall through to GoingDown default of stopping
+                }
+                return ControllerStates::Stopping
+            }
+        }
+        self.state.clone() //Don't transition states 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?;
+        }
+        else {
+            self.maintain_state().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?;
+            trace!("Got command {:?}",cmd);
+            let new_s = self.handle_cmd(&cmd);
+            trace!("State current {:?} new {:?}", self.state, new_s);
+            self.transition_state(&self.state.clone(), &new_s).await?
+        }
+    }
+
+}