Refactor motor and encoder modules for unit testing

src/hal/encoders.rs

@@ -1,24 +1,55 @@
 use rp2040_hal as hal;
-use hal::pio::{UninitStateMachine, StateMachine, PIO, PinDir, Rx, InstalledProgram, ValidStateMachine};
+#[cfg(not(test))]
+use hal::pio::{UninitStateMachine, StateMachine, PIO, PinDir, InstalledProgram};
+use hal::pio::{Rx, ValidStateMachine};
+#[cfg(not(test))]
 use hal::gpio::{Pin, FunctionPio0, PullUp, bank0::{Gpio8, Gpio9, Gpio12, Gpio13}};
+#[cfg(not(test))]
 use hal::pac::PIO0;
+#[cfg(not(test))]
 use defmt::info;
+#[cfg(not(test))]
 use pio_proc::pio_asm;
 
+#[cfg(test)]
+use self::mocks::MockRxHandle;
+
 pub enum MotorId {
     Left,
     Right,
 }
 
+pub trait EncoderRx {
+    fn read(&mut self) -> Option<u32>;
+}
+
+impl<T: ValidStateMachine> EncoderRx for Rx<T> {
+    fn read(&mut self) -> Option<u32> {
+        self.read()
+    }
+}
+
 pub struct Encoders {
+    #[cfg(not(test))]
     _program: InstalledProgram<PIO0>,
+    #[cfg(not(test))]
     _left_sm: StateMachine<(PIO0, hal::pio::SM0), hal::pio::Running>,
+    #[cfg(not(test))]
     _right_sm: StateMachine<(PIO0, hal::pio::SM1), hal::pio::Running>,
+
+    #[cfg(not(test))]
     left_rx: Rx<(PIO0, hal::pio::SM0)>,
+    #[cfg(test)]
+    pub left_rx: MockRxHandle,
+
+    #[cfg(not(test))]
     right_rx: Rx<(PIO0, hal::pio::SM1)>,
+    #[cfg(test)]
+    pub right_rx: MockRxHandle,
 }
 
 impl Encoders {
+    #[cfg(not(test))]
     pub fn new(
         pio: &mut PIO<PIO0>,
         sm0: UninitStateMachine<(PIO0, hal::pio::SM0)>,
@@ -108,9 +139,17 @@ impl Encoders {
         }
     }
 
-    /// Read the latest count from the PIO. 
+    #[cfg(test)]
+    pub fn new_test() -> Self {
+        Self {
+            left_rx: MockRxHandle::new(),
+            right_rx: MockRxHandle::new(),
+        }
+    }
+
+    /// Read the latest count from the PIO.
     /// This is non-blocking and returns the most recent value in the FIFO.
-    fn read_latest<T: ValidStateMachine>(rx: &mut Rx<T>) -> i32 {
+    fn read_latest(rx: &mut impl EncoderRx) -> i32 {
         let mut count = None;
         // TODO: use a match method here in order to avoid blocking
         while let Some(val) = rx.read() {
@@ -125,3 +164,64 @@ impl Encoders {
         (-left, -right)
     }
 }
+
+#[cfg(test)]
+pub mod mocks {
+    use super::*;
+    use std::cell::RefCell;
+    use std::rc::Rc;
+    use std::collections::VecDeque;
+
+    #[derive(Clone)]
+    pub struct MockRxHandle(pub Rc<RefCell<VecDeque<u32>>>);
+
+    impl MockRxHandle {
+        pub fn new() -> Self {
+            Self(Rc::new(RefCell::new(VecDeque::new())))
+        }
+
+        pub fn push(&self, value: u32) {
+            self.0.borrow_mut().push_back(value);
+        }
+    }
+
+    impl EncoderRx for MockRxHandle {
+        fn read(&mut self) -> Option<u32> {
+            self.0.borrow_mut().pop_front()
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_get_counts() {
+        let mut encoders = Encoders::new_test();
+        let left_rx = encoders.left_rx.clone();
+        let right_rx = encoders.right_rx.clone();
+
+        // Nothing to read
+        let (left, right) = encoders.get_counts();
+        assert_eq!(left, 0);
+        assert_eq!(right, 0);
+
+        // Push some data
+        left_rx.push(100);
+        left_rx.push(101); // Last one is 101
+
+        right_rx.push(50);
+        right_rx.push(51); // Last one is 51
+
+        let (left, right) = encoders.get_counts();
+        // get_counts returns negative values
+        assert_eq!(left, -101);
+        assert_eq!(right, -51);
+
+        // FIFO drained
+        let (left, right) = encoders.get_counts();
+        assert_eq!(left, 0);
+        assert_eq!(right, 0);
+    }
+}

src/hal/motors.rs

@@ -1,21 +1,59 @@
 use rp2040_hal as hal;
 use hal::pwm::{Pwm7, Slice, FreeRunning};
+#[cfg(not(test))]
 use hal::gpio::{Pin, FunctionSio, SioOutput, PullDown, bank0::{Gpio10, Gpio11}};
 use embedded_hal::PwmPin;
 use embedded_hal::digital::v2::OutputPin;
 
+#[cfg(test)]
+use self::mocks::{MockPwmHandle, MockPinHandle};
+
 pub enum MotorId {
     Left,
     Right,
 }
 
+pub trait PwmSlice {
+    fn set_ph_correct(&mut self);
+    fn enable(&mut self);
+    fn set_duty_a(&mut self, duty: u16);
+    fn set_duty_b(&mut self, duty: u16);
+}
+
+impl PwmSlice for Slice<Pwm7, FreeRunning> {
+    fn set_ph_correct(&mut self) {
+        self.set_ph_correct();
+    }
+    fn enable(&mut self) {
+        self.enable();
+    }
+    fn set_duty_a(&mut self, duty: u16) {
+        self.channel_a.set_duty(duty);
+    }
+    fn set_duty_b(&mut self, duty: u16) {
+        self.channel_b.set_duty(duty);
+    }
+}
+
 pub struct Motors {
+    #[cfg(not(test))]
     pwm: Slice<Pwm7, FreeRunning>,
+    #[cfg(test)]
+    pub pwm: MockPwmHandle,
+
+    #[cfg(not(test))]
     left_dir: Pin<Gpio11, FunctionSio<SioOutput>, PullDown>,
+    #[cfg(test)]
+    pub left_dir: MockPinHandle,
+
+    #[cfg(not(test))]
     right_dir: Pin<Gpio10, FunctionSio<SioOutput>, PullDown>,
+    #[cfg(test)]
+    pub right_dir: MockPinHandle,
 }
 
 impl Motors {
+    #[cfg(not(test))]
     pub fn new(
         mut pwm: Slice<Pwm7, FreeRunning>,
         left_dir: Pin<Gpio11, FunctionSio<SioOutput>, PullDown>,
@@ -35,6 +73,22 @@ impl Motors {
         m
     }
 
+    #[cfg(test)]
+    pub fn new_test() -> Self {
+        let mut pwm = MockPwmHandle::new();
+        // Initialize as in real new()
+        pwm.set_ph_correct();
+        pwm.enable();
+
+        let mut m = Self {
+            pwm,
+            left_dir: MockPinHandle::new(),
+            right_dir: MockPinHandle::new(),
+        };
+        m.stop();
+        m
+    }
+
     pub fn set_speed(&mut self, motor: MotorId, speed: f32) {
         let speed = speed.clamp(-1.0, 1.0);
         let abs_speed = if speed < 0.0 { -speed } else { speed };
@@ -48,7 +102,7 @@ impl Motors {
                 } else {
                     let _ = self.left_dir.set_low();
                 }
-                self.pwm.channel_b.set_duty(duty);
+                self.pwm.set_duty_b(duty);
             }
             MotorId::Right => {
                 // Right Motor is on Channel A
@@ -57,13 +111,138 @@ impl Motors {
                 } else {
                     let _ = self.right_dir.set_low();
                 }
-                self.pwm.channel_a.set_duty(duty);
+                self.pwm.set_duty_a(duty);
             }
         }
     }
 
     pub fn stop(&mut self) {
-        self.pwm.channel_a.set_duty(0);
-        self.pwm.channel_b.set_duty(0);
+        self.pwm.set_duty_a(0);
+        self.pwm.set_duty_b(0);
+    }
+}
+
+#[cfg(test)]
+pub mod mocks {
+    use super::*;
+    use std::cell::RefCell;
+    use std::rc::Rc;
+
+    pub struct MockPwm {
+        pub duty_a: u16,
+        pub duty_b: u16,
+        pub ph_correct: bool,
+        pub enabled: bool,
+    }
+
+    impl MockPwm {
+        pub fn new() -> Self {
+            Self {
+                duty_a: 0,
+                duty_b: 0,
+                ph_correct: false,
+                enabled: false,
+            }
+        }
+    }
+
+    #[derive(Clone)]
+    pub struct MockPwmHandle(pub Rc<RefCell<MockPwm>>);
+
+    impl MockPwmHandle {
+        pub fn new() -> Self {
+            Self(Rc::new(RefCell::new(MockPwm::new())))
+        }
+    }
+
+    impl PwmSlice for MockPwmHandle {
+        fn set_ph_correct(&mut self) {
+            self.0.borrow_mut().ph_correct = true;
+        }
+        fn enable(&mut self) {
+            self.0.borrow_mut().enabled = true;
+        }
+        fn set_duty_a(&mut self, duty: u16) {
+            self.0.borrow_mut().duty_a = duty;
+        }
+        fn set_duty_b(&mut self, duty: u16) {
+            self.0.borrow_mut().duty_b = duty;
+        }
+    }
+
+    pub struct MockPin {
+        pub high: bool,
+    }
+
+    impl MockPin {
+        pub fn new() -> Self {
+            Self { high: false }
+        }
+    }
+
+    #[derive(Clone)]
+    pub struct MockPinHandle(pub Rc<RefCell<MockPin>>);
+
+    impl MockPinHandle {
+        pub fn new() -> Self {
+            Self(Rc::new(RefCell::new(MockPin::new())))
+        }
+    }
+
+    impl OutputPin for MockPinHandle {
+        type Error = ();
+        fn set_low(&mut self) -> Result<(), Self::Error> {
+            self.0.borrow_mut().high = false;
+            Ok(())
+        }
+        fn set_high(&mut self) -> Result<(), Self::Error> {
+            self.0.borrow_mut().high = true;
+            Ok(())
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_motors_initialization() {
+        let motors = Motors::new_test();
+
+        assert!(motors.pwm.0.borrow().ph_correct);
+        assert!(motors.pwm.0.borrow().enabled);
+        assert_eq!(motors.pwm.0.borrow().duty_a, 0); // stop() called in new
+        assert_eq!(motors.pwm.0.borrow().duty_b, 0);
+    }
+
+    #[test]
+    fn test_set_speed_left() {
+        let mut motors = Motors::new_test();
+
+        // Forward
+        motors.set_speed(MotorId::Left, 0.5);
+        assert!(motors.left_dir.0.borrow().high);
+        assert_eq!(motors.pwm.0.borrow().duty_b, 32767); // 0.5 * 65535
+
+        // Backward
+        motors.set_speed(MotorId::Left, -0.5);
+        assert!(!motors.left_dir.0.borrow().high);
+        assert_eq!(motors.pwm.0.borrow().duty_b, 32767);
+    }
+
+    #[test]
+    fn test_set_speed_right() {
+        let mut motors = Motors::new_test();
+
+        // Forward
+        motors.set_speed(MotorId::Right, 0.5);
+        assert!(motors.right_dir.0.borrow().high);
+        assert_eq!(motors.pwm.0.borrow().duty_a, 32767); // 0.5 * 65535
+
+        // Backward
+        motors.set_speed(MotorId::Right, -0.5);
+        assert!(!motors.right_dir.0.borrow().high);
+        assert_eq!(motors.pwm.0.borrow().duty_a, 32767);
     }
 }

src/lib.rs

@@ -1,2 +1,2 @@
-#![no_std]
+#![cfg_attr(not(test), no_std)]
 pub mod hal;