Added some tests, fixed mistakes tests found

gem-remotes-esp32/analyze-binary.py

@@ -0,0 +1,131 @@
+#!/bin/env python3
+
+'''
+Quick and dirty script to analyze the contents of the binary.
+
+Shows total size of a crate's added code, count of the number of sections,
+and the name of the crate.  Names with non-rust names are lumped into 
+esp-idf, as that's most likely what they are in this project.
+
+Unfortunately, since much of the actual functionality is carried out by the
+esp-idf, including newly added functionality (such as bluetooth) often mostly 
+just grows the amount of esp-idf that is included.
+'''
+
+import subprocess
+import os.path
+import os.getcwd
+
+# Shorten names to crates instead of the whole function
+mangle_name = True
+
+program_name = os.path.split(os.getcwd())[1]
+print(program_name)
+targets = ("./target/xtensa-esp32-espidf/release/", "./target/xtensa-esp32-espidf/debug/")
+
+def find_file():
+    for t in targets:
+        if os.path.isfile(t + program_name):
+            #return t
+            analyze_file(t + program_name)
+
+def analyze_file(f):
+    results = subprocess.run(["nm", "-S", "--demangle=rust", "--size-sort", f], capture_output=True).stdout
+    lines = results.splitlines()
+    data = {}
+    for line in lines:
+        cols = line.split()
+        # Cols are: 0: position, 1: size, 2: ? 3: name
+        if len(cols) < 4:
+            pass # this shouldn't happen if we sort by size; but nm lists things without a size otherwise.
+        else:
+            raw_name = cols[3].decode("utf-8")
+            raw_size = cols[1]
+            if mangle_name:
+                if len(raw_name):
+                    while "<" == raw_name[0] or "&" == raw_name[0]:
+                        raw_name = raw_name[1:]
+                    parts = raw_name.split(':')
+                    if len(parts[0]) == len(raw_name):
+                        # Assume if it has no crate delimiters that it is part of esp-idf
+                        name = "esp-idf"
+                    else:
+                        name = parts[0]
+                else:
+                    name = "(blank)"
+            else:
+                name = raw_name
+            size = int(raw_size, 16)
+            if name in data:
+                (count, total) = data[name]
+                count += 1
+                total += size
+                data[name] = (count, total)
+            else:
+                data[name] = (1, size)
+    print("  total | ct | crate")
+    sorted_data = []
+    for item in data.items():
+        (name, (count, size)) = item
+        sorted_data.append((size, count, name))
+    sorted_data.sort(key=lambda tup: tup[0])
+    for i in sorted_data:
+        (size, count, name) = i
+        print(f'{size:8,}', f'{count:4}', name)
+    i = 0
+    for tup in data.values():
+        i += tup[1]
+    print("\n","Total size: ", f'{i:,}', "Actual binary size may differ due to included data and the chunks nm didn't identify")
+
+
+def main():
+    find_file()
+
+if __name__ == "__main__":
+    main()
+
+'''
+For comparison; the 'Hello, World' app generated by 'cargo generate esp-rs/esp-idf-template cargo'
+
+Hello, World (release) analysis
+total | ct | crate
+       9    1 panic_abort
+      11    1 esp_idf_sys
+      84    1 hello_world
+     223    1 memchr
+     315   12 log
+     654    2 adler
+     724    8 esp_idf_svc
+   2,757   24 object
+   8,104    6 miniz_oxide
+  15,044  181 alloc
+  15,765   42 rustc_demangle
+  19,581   37 addr2line
+  26,343  200 std
+  33,966  291 core
+  34,980  104 gimli
+ 159,412 1778 esp-idf
+
+ Total size:  317,972
+
+Hello, World (debug) analysis
+  total | ct | crate
+       9    1 panic_abort
+      11    1 esp_idf_sys
+      84    1 hello_world
+     267    2 memchr
+     356   13 log
+     817    8 esp_idf_svc
+   1,012    5 adler
+   3,571   45 object
+   9,796   13 miniz_oxide
+  13,331   45 rustc_demangle
+  20,043   45 addr2line
+  28,044  281 std
+  35,175  629 alloc
+  38,761  210 gimli
+  60,658  863 core
+ 186,909 2286 esp-idf
+
+ Total size:  398,844
+ '''

gem-remotes-esp32/src/motor_driver.rs

@@ -27,8 +27,12 @@ impl MotorDriverDebug {
 
     pub async fn run(&self) -> Result<()> {
         loop {
-            let cmd = self.recv_q.recv().await.expect("Unexpected failure in motor driver command queue");
+            let cmd = self.recv_q.recv()
-            self.handle_cmd(cmd).await.expect("Unexpected failure of motor driver notification queue");
+                .await
+                .expect("Unexpected failure in motor driver command queue");
+            self.handle_cmd(cmd)
+                .await
+                .expect("Unexpected failure of motor driver notification queue");
         }
     }
 
@@ -41,20 +45,26 @@ impl MotorDriverDebug {
         Ok(())
     }
 
-    pub async fn start_up(&self) -> Result<()> {
+    async fn start_up(&self) -> Result<()> {
         warn!("Starting motor, direction: Up");
         Ok(())
     }
-    pub async fn start_down(&self) -> Result<()> {
+    async fn start_down(&self) -> Result<()> {
         warn!("Starting motor, direction: Down");
         Ok(())
     }
-    pub async fn stop(&self) -> Result<()> {
+    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.
+// 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.

gem-remotes-lib/Cargo.toml

@@ -1,3 +1,11 @@
+
+[profile.release]
+opt-level = "s"
+
+[profile.dev]
+debug = true    # Symbols are nice and they don't increase the size on Flash
+opt-level = "z"
+
 [package]
 name = "gem-remotes-lib"
 version = "0.1.0"
@@ -6,6 +14,7 @@ edition = "2021"
 [dependencies]
 anyhow = "1.0.86"
 async-channel = "2.3.1"
+async-io = "2.3.4"
 log = "0.4.22"
 strum = "0.26.3"
 strum_macros = "0.26.4"

gem-remotes-lib/src/commands.rs

@@ -15,9 +15,9 @@ pub enum Commands {
     PicRecvUp {data: Button},
     PicRecvDown {data: Button},
     PicRecvStop {data: Button},
-    PicRecvLimitUp {data: Button},   // 0 for not hit, 1 for hit
+    PicRecvLimitUp {data: Button}, 
-    PicRecvLimitDown {data: Button}, // 0 for not hit, 1 for hit
+    PicRecvLimitDown {data: Button},
-    PicRecvAutoMode {data: Button},  // 0 for disallowed, 1 for allowed
+    PicRecvAutoMode {data: Toggle},  // 0 for disallowed, 1 for allowed
 
     // 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.
 
@@ -44,6 +44,8 @@ pub enum Commands {
     NotifyMotorStop {data: Button},
 
     EraseBleBonds,
+
+    TestingExit, // Used only in unit/integration tests.  Do not subscribe for.
 }
 
 #[derive(Copy, Clone, Debug)]
@@ -52,6 +54,13 @@ pub enum Button {
     Pressed =1
 }
 
+// Distinguish toggles(like auto) which is on/off from buttons (which are pressed/released)
+#[derive(Copy, Clone, Debug)]
+pub enum Toggle {
+    Inactive = 0,
+    Active =1
+}
+
 pub type CmdType = std::mem::Discriminant<Commands>;
 
 /// Consider commands equal if they have the same command type, but different values

gem-remotes-lib/src/lib.rs

@@ -27,19 +27,16 @@ pub use dispatch::{
 
 
 
-// Test Code for whole module
+// // Test Code for whole module // //////////////////////////////////////////////////////////////
-
-pub fn add(left: u64, right: u64) -> u64 {
-    left + right
-}
 
 #[cfg(test)]
 mod tests {
-    use super::*;
+    //use super::*;
 
     #[test]
-    fn it_works() {
+    fn empty_test() {
-        let result = add(2, 2);
+        assert_eq!(1, 1);
-        assert_eq!(result, 4);
     }
 }
+
+// TODO: Check whole module for panics (unwrap, expect, panic) and ensure that it is appropriate

gem-remotes-lib/src/motor_controller.rs

@@ -2,9 +2,9 @@
 /// command messages.
 
 use log::*; //{trace, debug, info, warn, error}
-use anyhow::Result;
+use anyhow::{anyhow, Result};
 use async_channel::{Receiver, Sender};
-use crate::commands::{Commands, Button};
+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.
@@ -18,8 +18,7 @@ enum ControllerStates {
     AutoDown,
 }
 
-
+#[derive(Clone, Copy, Debug, PartialEq)]
-#[derive(Clone, Copy, Debug)]
 pub enum MotorCommands {
     StartUp,
     StartDown,
@@ -76,7 +75,7 @@ impl Controller {
             Commands::BluetoothStop{data: Button::Released},
             Commands::PicRecvLimitUp{data: Button::Released},
             Commands::PicRecvLimitDown{data: Button::Released},
-            Commands::PicRecvAutoMode{data: Button::Released},
+            Commands::PicRecvAutoMode{data: Toggle::Inactive},
             Commands::StopTimerExpired,
             Commands::ButtonTimerExpired,
         ];
@@ -155,34 +154,35 @@ impl Controller {
     }
 
     /// Determines the state the controller should be in based on the command received.
-    async fn handle_cmd(&mut self, cmd: &Commands) -> ControllerStates {
+    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 => {return self.change_state_if_pressed(data, self.remote_up_or_auto_up())}
+                    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");
-                        return self.change_state_if_released(data, ControllerStates::Stopping)
+                        rc =  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)
+                        rc= 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::Stopped => {rc = 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)
+                        rc = 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)
+                        rc = self.change_state_if_released(data, ControllerStates::Stopping)
                     }
                     ControllerStates::AutoDown => {}
                 }
@@ -195,7 +195,7 @@ impl Controller {
                     ControllerStates::AutoUp | 
                     ControllerStates::GoingDown |
                     ControllerStates::AutoDown => {
-                        return self.change_state_if_pressed(data, ControllerStates::Stopping)
+                        rc = self.change_state_if_pressed(data, ControllerStates::Stopping)
                     }
                 }
             }
@@ -207,13 +207,13 @@ impl Controller {
                     ControllerStates::GoingUp | 
                     ControllerStates::AutoUp=> {
                         released_warning(data, "Limit switches may be installed incorrectly!");
-                        return self.change_state_if_pressed(data, ControllerStates::Stopping)
+                        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.
-                        return self.change_state_if_pressed(data, ControllerStates::Stopping)
+                        rc = self.change_state_if_pressed(data, ControllerStates::Stopping)
                     } 
                 }
             }
@@ -226,20 +226,30 @@ impl Controller {
                     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)
+                        rc = 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)
+                        rc = self.change_state_if_pressed(data, ControllerStates::Stopping)
                     }
                 }
             }
-            Commands::PicRecvAutoMode { data } => {self.set_auto(data);}
+            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 => {return ControllerStates::Stopped}
+                    ControllerStates::Stopping => {rc = ControllerStates::Stopped}
                     ControllerStates::GoingUp |
                     ControllerStates::AutoUp |
                     ControllerStates::GoingDown |
@@ -252,9 +262,9 @@ impl Controller {
                 match self.state {
                     ControllerStates::Stopped => {}
                     ControllerStates::Stopping => {}
-                    ControllerStates::GoingUp => {return ControllerStates::Stopping}
+                    ControllerStates::GoingUp => {rc = ControllerStates::Stopping}
                     ControllerStates::AutoUp => {}
-                    ControllerStates::GoingDown => {return ControllerStates::Stopping}
+                    ControllerStates::GoingDown => {rc = ControllerStates::Stopping}
                     ControllerStates::AutoDown => {}
                 }
             }
@@ -277,8 +287,12 @@ impl Controller {
             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
+    //self.state.clone() // Don't transition by default
+    Ok(rc)
     }
 
     async fn transition_state(&mut self, old_s: &ControllerStates, new_s: &ControllerStates) -> Result<()> {
@@ -298,10 +312,17 @@ impl Controller {
         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;
+            match self.handle_cmd(&cmd).await {
-            trace!("State current {:?} new {:?}", self.state, new_s);
+                Ok(new_s) => {
-            self.transition_state(&self.state.clone(), &new_s).await.expect("Unexpected state change failure in motor controller");
+                    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 {
@@ -356,12 +377,12 @@ impl Controller {
         }
     }
 
-    fn set_auto(&mut self, data: &Button) {
+    fn set_auto(&mut self, data: &Toggle) {
         match data {
-            Button::Released => {
+            Toggle::Inactive => {
                 self.auto_mode = AutoMode::Disallowed;
             }
-            Button::Pressed => {
+            Toggle::Active => {
                 if self.limit_state == LimitState::BothHit {
                     warn!("Limit switches not detected.  Aborting auto  mode.");
                 } else {
@@ -371,6 +392,7 @@ impl Controller {
         }
     }
 
+    // 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 => {
@@ -430,16 +452,315 @@ impl Controller {
 
 }
 
+// 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 intenral
+        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 intenral
+        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(())
+    }
+}