CTL - Detect Brakes Actuation with ToF (#70)

Co-authored-by: David Beechey <[email protected]>

boards/stm32l476rg/src/bin/pneumatics_control_test.rs

@@ -1,14 +1,25 @@
 #![no_std]
 #![no_main]
 
+use core::cell::RefCell;
+
 use embassy_executor::Spawner;
 use embassy_stm32::gpio::Output;
 use embassy_stm32::gpio::{Level, Speed};
+use embassy_stm32::i2c::I2c;
+use embassy_stm32::mode::Blocking;
+use embassy_stm32::time::Hertz;
+use embassy_sync::blocking_mutex::raw::NoopRawMutex;
+use embassy_sync::blocking_mutex::Mutex;
 use embassy_time::{Duration, Timer};
-use hyped_boards_stm32l476rg::io::Stm32l476rgGpioOutput;
+use hyped_boards_stm32l476rg::io::{Stm32l476rgGpioOutput, Stm32l476rgI2c};
 use hyped_control::pneumatics::Pneumatics;
+use hyped_sensors::time_of_flight::{TimeOfFlight, TimeOfFlightAddresses};
+use static_cell::StaticCell;
 use {defmt_rtt as _, panic_probe as _};
 
+type I2c1Bus = Mutex<NoopRawMutex, RefCell<I2c<'static, Blocking>>>;
+
 #[embassy_executor::main]
 async fn main(_spawner: Spawner) -> ! {
     let p = embassy_stm32::init(Default::default());
@@ -19,16 +30,42 @@ async fn main(_spawner: Spawner) -> ! {
     let suspension_solenoid_pin =
         Stm32l476rgGpioOutput::new(Output::new(p.PA2, Level::Low, Speed::Low));
 
+    // Create I2C bus for the time-of-flight sensor
+    let i2c = I2c::new_blocking(p.I2C1, p.PB8, p.PB9, Hertz(100_000), Default::default());
+    static I2C_BUS: StaticCell<I2c1Bus> = StaticCell::new();
+    let i2c_bus = I2C_BUS.init(Mutex::new(RefCell::new(i2c)));
+
+    let mut hyped_i2c = Stm32l476rgI2c::new(i2c_bus);
+
+    // Create time-of-flight sensor
+    let time_of_flight_sensor = TimeOfFlight::new(
+        &mut hyped_i2c,
+        TimeOfFlightAddresses::Address29,
+    )
+    .expect(
+        "Failed to create Time of Flight sensor. Check the wiring and I2C address of the sensor.",
+    );
+
     // Create pneumatics control object
-    let mut pneumatics = Pneumatics::new(brakes_solenoid_pin, suspension_solenoid_pin);
+    let mut pneumatics = Pneumatics::new(
+        brakes_solenoid_pin,
+        suspension_solenoid_pin,
+        time_of_flight_sensor,
+    )
+    .await
+    .expect("Failed to create pneumatics control object.");
 
     loop {
         // Simple test to open and close the brakes solenoid
         defmt::info!("Opening brakes solenoid...");
-        pneumatics.engage_brakes();
+        pneumatics.engage_brakes().await.expect(
+            "Failed to engage brakes. Check the wiring and GPIO pin of the brakes solenoid.",
+        );
         Timer::after(Duration::from_millis(1000)).await;
         defmt::info!("Closing brakes solenoid...");
-        pneumatics.disengage_brakes();
+        pneumatics.disengage_brakes().await.expect(
+            "Failed to disengage brakes. Check the wiring and GPIO pin of the brakes solenoid.",
+        );
         Timer::after(Duration::from_millis(1000)).await;
 
         // Simple test to open and close the suspension solenoid

lib/control/Cargo.toml

@@ -4,7 +4,11 @@ version = "0.1.0"
 edition = "2021"
 
 [dependencies]
+embassy-time = { version = "0.3.1", git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"}
+embassy-sync = { version = "0.6.0", git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"}
 heapless = "0.8.0"
 
 hyped_core = { path = "../core" }
 hyped_gpio = { path = "../io/hyped_gpio" }
+hyped_i2c = { path = "../io/hyped_i2c"}
+hyped_sensors = { path = "../sensors"}  

lib/control/src/lib.rs

@@ -1,4 +1,4 @@
-#![no_std]
+#![cfg_attr(not(test), no_std)]
 
 pub mod hp_relay;
 pub mod pneumatics;

lib/control/src/pneumatics.rs

@@ -1,4 +1,11 @@
+use embassy_time::with_timeout;
+use embassy_time::Duration;
 use hyped_gpio::HypedGpioOutputPin;
+use hyped_i2c::HypedI2c;
+use hyped_sensors::{
+    time_of_flight::{TimeOfFlight, TimeOfFlightError},
+    SensorValueRange,
+};
 
 #[derive(Debug, PartialEq, Clone, Copy)]
 pub enum BrakeState {
@@ -12,46 +19,86 @@ pub enum LateralSuspensionState {
     Retracted,
 }
 
+#[derive(Debug)]
+pub enum BrakeActuationFailure {
+    TimeOfFlightError(TimeOfFlightError),
+    SensorNotInTolerance,
+    TimeoutError,
+}
+
+const BRAKE_ACTUATION_THRESHOLD: u8 = 5; // TODOLater: replace with whatever value indicates the brakes are actuated. > this value means brakes are not actuated.
+const BRAKE_CHECK_TIMEOUT: Duration = Duration::from_millis(100);
+
 /// Represents the pneumatics systems (brakes and lateral suspension) of the pod.
 /// Outputs two GPIO signals, one for the brakes and one for the lateral suspension, which turn on/off a solenoid valve.
-pub struct Pneumatics<P: HypedGpioOutputPin> {
+pub struct Pneumatics<'a, P: HypedGpioOutputPin, T: HypedI2c> {
     brake_state: BrakeState,
     lateral_suspension_state: LateralSuspensionState,
     brake_pin: P,
     lateral_suspension_pin: P,
+    time_of_flight: TimeOfFlight<'a, T>,
 }
 
-impl<P: HypedGpioOutputPin> Pneumatics<P> {
-    pub fn new(brake_pin: P, lateral_suspension_pin: P) -> Self {
+impl<'a, P: HypedGpioOutputPin, T: HypedI2c> Pneumatics<'a, P, T> {
+    pub async fn new(
+        brake_pin: P,
+        lateral_suspension_pin: P,
+        time_of_flight: TimeOfFlight<'a, T>,
+    ) -> Result<Self, BrakeActuationFailure> {
         let mut pneumatics = Pneumatics {
             brake_state: BrakeState::Engaged,
             lateral_suspension_state: LateralSuspensionState::Retracted,
             brake_pin,
             lateral_suspension_pin,
+            time_of_flight,
         };
 
         // Engage brakes and retract lateral suspension on startup
-        pneumatics.engage_brakes();
         pneumatics.retract_lateral_suspension();
-        pneumatics
+        match pneumatics.engage_brakes().await {
+            Ok(_) => Ok(pneumatics),
+            Err(e) => Err(e),
+        }
     }
 
     /// Engages the brakes by setting the brake GPIO pin to low.
-    pub fn engage_brakes(&mut self) {
-        self.brake_state = BrakeState::Engaged;
-
+    /// After actuating the brakes, the time of flight sensor is checked to ensure the brakes have been actuated.
+    /// If the brakes have not been actuated within the timeout period, a timeout error is returned.
+    pub async fn engage_brakes(&mut self) -> Result<(), BrakeActuationFailure> {
         // Brake pin is set to low, as brakes clamp with no power,
         // and are retracted when powered.
         self.brake_pin.set_low();
+
+        // Check that the brakes have been actuated by reading the time of flight sensor.
+        match with_timeout(BRAKE_CHECK_TIMEOUT, self.check_brake_actuation()).await {
+            Ok(Ok(_)) => {
+                self.brake_state = BrakeState::Engaged;
+                Ok(())
+            }
+            Ok(Err(e)) => Err(e),
+            // If the brakes have not been actuated within the timeout period, return a timeout error.
+            Err(_) => Err(BrakeActuationFailure::TimeoutError),
+        }
     }
 
     /// Disengages the brakes by setting the brake GPIO pin to high.
-    pub fn disengage_brakes(&mut self) {
-        self.brake_state = BrakeState::Disengaged;
-
+    /// After disengaging the brakes, the time of flight sensor is checked to ensure the brakes have been disengaged.
+    /// If the brakes have not been disengaged within the timeout period, a timeout error is returned.
+    pub async fn disengage_brakes(&mut self) -> Result<(), BrakeActuationFailure> {
         // Brake pin is set to high, as brakes retract when powered,
         // and are retracted when powered.
         self.brake_pin.set_high();
+
+        // Check that the brakes have been disengaged by reading the time of flight sensor.
+        match with_timeout(BRAKE_CHECK_TIMEOUT, self.check_brake_actuation()).await {
+            Ok(Ok(_)) => {
+                self.brake_state = BrakeState::Disengaged;
+                Ok(())
+            }
+            Ok(Err(e)) => Err(e),
+            // If the brakes have not been disengaged within the timeout period, return a timeout error.
+            Err(_) => Err(BrakeActuationFailure::TimeoutError),
+        }
     }
 
     /// Deploys the lateral suspension by setting the lateral suspension GPIO pin to high.
@@ -73,47 +120,25 @@ impl<P: HypedGpioOutputPin> Pneumatics<P> {
     pub fn get_lateral_suspension_state(&self) -> LateralSuspensionState {
         self.lateral_suspension_state
     }
-}
 
-#[cfg(test)]
-mod tests {
-    use super::*;
-    use hyped_gpio::mock_gpio::MockGpioOutputPin;
-
-    #[test]
-    fn test_pneumatics() {
-        let brake_pin = MockGpioOutputPin::new();
-        let lateral_suspension_pin = MockGpioOutputPin::new();
-
-        let mut pneumatics = Pneumatics::new(brake_pin, lateral_suspension_pin);
-
-        // Check that the brakes are engaged and the lateral suspension is retracted on startup
-        assert_eq!(pneumatics.get_brake_state(), BrakeState::Engaged);
-        assert_eq!(
-            pneumatics.get_lateral_suspension_state(),
-            LateralSuspensionState::Retracted
-        );
-
-        // Disengage brakes
-        pneumatics.disengage_brakes();
-        assert_eq!(pneumatics.get_brake_state(), BrakeState::Disengaged);
-
-        // Engage brakes
-        pneumatics.engage_brakes();
-        assert_eq!(pneumatics.get_brake_state(), BrakeState::Engaged);
-
-        // Deploy lateral suspension
-        pneumatics.deploy_lateral_suspension();
-        assert_eq!(
-            pneumatics.get_lateral_suspension_state(),
-            LateralSuspensionState::Deployed
-        );
-
-        // Retract lateral suspension
-        pneumatics.retract_lateral_suspension();
-        assert_eq!(
-            pneumatics.get_lateral_suspension_state(),
-            LateralSuspensionState::Retracted
-        );
+    async fn check_brake_actuation(&mut self) -> Result<(), BrakeActuationFailure> {
+        let sensor_result = self.time_of_flight.single_shot_measurement();
+
+        let sensor_range = match sensor_result {
+            Ok(v) => v,
+            Err(e) => return Err(BrakeActuationFailure::TimeOfFlightError(e)),
+        };
+
+        let sensor_value = match sensor_range {
+            SensorValueRange::Safe(s) => s,
+            SensorValueRange::Warning(w) => w,
+            SensorValueRange::Critical(c) => c,
+        };
+
+        if sensor_value >= BRAKE_ACTUATION_THRESHOLD {
+            return Err(BrakeActuationFailure::SensorNotInTolerance);
+        }
+
+        Ok(())
     }
 }

lib/sensors/src/time_of_flight.rs

@@ -1,6 +1,7 @@
 use crate::SensorValueRange;
 use defmt;
 use hyped_i2c::{i2c_write_or_err_16, HypedI2c, I2cError};
+
 /// time_of_flight implements the logic to read Time of Flight data from the VL6180V1 Time of Flight
 /// sensor using I2C peripheral provided by the Hyped I2c trait.
 ///
@@ -30,6 +31,7 @@ impl<'a, T: HypedI2c> TimeOfFlight<'a, T> {
     ) -> Result<Self, TimeOfFlightError> {
         Self::new_with_bounds(i2c, device_address, default_calculate_bounds)
     }
+
     /// Create a new instance of the time of flight sensor and configure it with bounds involved.
     pub fn new_with_bounds(
         i2c: &'a mut T,