adds the code used for the water demo

flight/src/drive/task.rs

@@ -2,9 +2,10 @@ use std::any::type_name;
 use std::fmt::{Debug, Formatter};
 use std::sync::mpsc::Receiver;
 use std::time::Instant;
-use log::{error, info, trace, warn};
+use log::{error, info, trace};
+use crate::commanded_state::CommandedState;
 use crate::drive::Drive;
-use crate::hardware::a4963::A4963;
+use crate::hardware::a4963::{Direction, A4963};
 use crate::hardware::pwm::PwmOutput;
 use crate::scheduler::{CyclicTask, TaskHandle};
 
@@ -15,10 +16,16 @@ const STARTUP_THROTTLE: f64 = 0.5;
 const MAXIMUM_THROTTLE: f64 = 1.0;
 
 const STARTUP_TIME: u16 = 5; // 5 cycles = 0.5 second
+const REVERSE_OFF_TIME: u16 = 10; // 10 cycles = 1.0 second
+const ERROR_TIME: u16 = 10; // 10 cycles = 1.0 second
 
 #[derive(Clone, Debug)]
 pub enum DriveMessage {
-    SetThrust(f64),
+    SetThrust {
+        throttle: f64,
+        valid_until: Instant,
+        priority: u8
+    },
 }
 
 impl<D: Debug> Drive for TaskHandle<DriveMessage, D> {
@@ -27,14 +34,18 @@ impl<D: Debug> Drive for TaskHandle<DriveMessage, D> {
             "TaskHandle<DriveMessage, D>::set_pin(self: {self:?}, throttle: {throttle}, valid_until: {valid_until:?}, priority: {priority})"
         );
         // This can only fail if the other side is disconnected which we want to ignore
-        let _ = self.sender.send(DriveMessage::SetThrust(throttle));
+        let _ = self.sender.send(DriveMessage::SetThrust {
+            throttle,
+            valid_until,
+            priority,
+        });
     }
 }
 
 pub struct DriveTask<MotorController, Pwm> {
     motor_controller: MotorController,
     pwm: Pwm,
-    thrust_setpoint: f64,
+    thrust_setpoint: CommandedState<f64>,
     state: State,
 }
 
@@ -42,7 +53,7 @@ impl<MotorController, Pwm> Debug for DriveTask<MotorController, Pwm> {
     fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
         write!(
             f,
-            "DriveTask<MotorController={}, Pwm={}> {{ thrust_setpoint: {}, state: {:?} }}",
+            "DriveTask<MotorController={}, Pwm={}> {{ thrust_setpoint: {:?}, state: {:?} }}",
             type_name::<MotorController>(),
             type_name::<Pwm>(),
             self.thrust_setpoint,
@@ -53,13 +64,21 @@ impl<MotorController, Pwm> Debug for DriveTask<MotorController, Pwm> {
 
 #[derive(Clone, Debug)]
 enum State {
-    Off,
+    Off {
+        timer: u16,
+        from_direction: Option<Direction>,
+    },
     Configure,
     Startup {
         timer: u16,
+        direction: Direction,
+    },
+    Operational {
+        direction: Direction,
+    },
+    Error {
+        timer: u16,
     },
-    Operational,
-    Error,
 }
 
 impl<MotorController, Pwm> DriveTask<MotorController, Pwm>
@@ -71,40 +90,59 @@ where
         Self {
             motor_controller,
             pwm,
-            thrust_setpoint: OFF_THROTTLE,
-            state: State::Off,
+            thrust_setpoint: CommandedState::new(OFF_THROTTLE),
+            state: State::Off {
+                timer: 0,
+                from_direction: None,
+            },
         }
     }
 
     fn step_state(&mut self, state: State) -> State {
         trace!("DriveTask::step_state(self: {self:?}, state: {state:?})");
         match state {
-            State::Error => {
+            State::Error { timer } => {
                 // A generic error state which cleans up
                 // any internals
-                self.thrust_setpoint = OFF_THROTTLE;
-
-                State::Off
+                if timer < ERROR_TIME {
+                    State::Error {
+                        timer: timer.saturating_add(1),
+                    }
+                } else {
+                    State::Off {
+                        timer: 0,
+                        from_direction: None
+                    }
+                }
             },
-            State::Off => {
+            State::Off { timer, from_direction } => {
                 if let Err(err) = self.pwm.set_duty_cycle(OFF_THROTTLE) {
                     error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}");
                     return state;
                 }
 
-                if self.thrust_setpoint >= THRESHOLD_THROTTLE {
-                    // Immediately start performing the startup
-                    return self.step_state(State::Configure);
+                if self.thrust_setpoint.abs() >= THRESHOLD_THROTTLE {
+                    let desired_direction = if *self.thrust_setpoint > 0.0 { Direction::Forward } else {Direction::Reverse};
+                    // Either the direction matches, or we don't have a direction
+                    let matching_direction = from_direction.map(|from| from == desired_direction).unwrap_or(true);
+                    let can_reverse = timer > REVERSE_OFF_TIME;
+                    if can_reverse || matching_direction {
+                        // Immediately start performing the startup
+                        return self.step_state(State::Configure);
+                    }
                 }
 
-                state
+                State::Off {
+                    timer: timer.saturating_add(1),
+                    from_direction
+                }
             }
             State::Configure => {
                 let diagnostic = match self.motor_controller.check_health() {
                     Ok(diagnostic) => diagnostic,
                     Err(err) => {
                         error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Failed to check device health: {err}");
-                        return self.step_state(State::Error);
+                        return self.step_state(State::Error { timer: 0 });
                     }
                 };
                 if let Err(err) = diagnostic.assert_consistency() {
@@ -117,44 +155,62 @@ where
                     return state;
                 }
 
-                match self.motor_controller.write_configuration() {
+                let direction = if *self.thrust_setpoint > 0.0 { Direction::Forward } else { Direction::Reverse };
+                match self.motor_controller.write_configuration(direction) {
                     Ok(()) => State::Startup {
                         timer: 0,
+                        direction,
                     },
                     Err(err) => {
                         error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Failed to write motor configuration: {err}");
-                        self.step_state(State::Error)
+                        self.step_state(State::Error { timer: 0 })
                     },
                 }
             }
-            State::Startup { timer } => {
-                if self.thrust_setpoint < THRESHOLD_THROTTLE {
-                    return self.step_state(State::Off);
+            State::Startup { timer, direction } => {
+                let thrust_setpoint = match direction {
+                    Direction::Forward => *self.thrust_setpoint,
+                    Direction::Reverse => -*self.thrust_setpoint,
+                };
+                if thrust_setpoint < THRESHOLD_THROTTLE {
+                    return self.step_state(State::Off  { timer: 0, from_direction: Some(direction) });
                 }
-                if let Err(err) = self.pwm.set_duty_cycle(STARTUP_THROTTLE) {
+                if let Err(err) = self.pwm.set_duty_cycle(
+                    match direction {
+                        Direction::Forward => STARTUP_THROTTLE,
+                        Direction::Reverse => -STARTUP_THROTTLE,
+                    }
+                ) {
                     error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}");
-                    return self.step_state(State::Error);
+                    return self.step_state(State::Error { timer: 0 });
                 }
 
                 // remain in the current state
                 if timer < STARTUP_TIME {
                     State::Startup {
-                        timer: timer.saturating_add(1)
+                        timer: timer.saturating_add(1),
+                        direction
                     }
                 } else {
-                    State::Operational
+                    State::Operational {
+                        direction
+                    }
                 }
             }
-            State::Operational => {
-                if self.thrust_setpoint < THRESHOLD_THROTTLE {
-                    return self.step_state(State::Off);
+            State::Operational { direction } => {
+                let thrust_setpoint = match direction {
+                    Direction::Forward => *self.thrust_setpoint,
+                    Direction::Reverse => -*self.thrust_setpoint,
+                };
+                if thrust_setpoint < THRESHOLD_THROTTLE {
+                    return self.step_state(State::Off { timer: 0, from_direction: Some(direction)});
                 }
 
                 let diagnostic = match self.motor_controller.check_health() {
                     Ok(diagnostic) => diagnostic,
                     Err(err) => {
                         error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Failed to check device health: {err}");
-                        return self.step_state(State::Error);
+                        return self.step_state(State::Error { timer: 0 });
                     }
                 };
                 if let Err(err) = diagnostic.assert_consistency() {
@@ -164,22 +220,22 @@ where
                     if diagnostic.loss_of_synchronization {
                         if let Err(err) = self.pwm.set_duty_cycle(OFF_THROTTLE) {
                             error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}");
-                            return State::Error;
+                            return State::Error { timer: 0 };
                         }
-                        warn!("Loss of sync");
-                        return State::Startup { timer: 0 };
+                        info!("Drive Loss of Sync - Repeating Startup");
+                        return State::Startup { timer: 0, direction };
                     }
                     error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Diagnostic: {diagnostic:?}");
-                    return State::Error;
+                    return State::Error { timer: 0 };
                 }
 
-                let new_thrust = self.thrust_setpoint.clamp(MINIMUM_THROTTLE, MAXIMUM_THROTTLE);
+                let new_thrust = thrust_setpoint.clamp(MINIMUM_THROTTLE, MAXIMUM_THROTTLE);
                 if let Err(err) = self.pwm.set_duty_cycle(new_thrust) {
                     error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}");
-                    return self.step_state(State::Error);
+                    return self.step_state(State::Error { timer: 0 });
                 }
 
-                State::Operational
+                State::Operational { direction }
             }
         }
     }
@@ -200,9 +256,15 @@ where
     fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant) {
         trace!("DriveTask::step(self: {self:?}, receiver: {receiver:?}, step_time: {step_time:?})");
 
+        self.thrust_setpoint.evaluate(step_time);
+
         while let Ok(message) = receiver.try_recv() {
             match message {
-                DriveMessage::SetThrust(new_thrust) => self.thrust_setpoint = new_thrust,
+                DriveMessage::SetThrust { throttle, valid_until, priority } => self.thrust_setpoint.insert(
+                    throttle.clamp(-MAXIMUM_THROTTLE, MAXIMUM_THROTTLE),
+                    valid_until,
+                    priority,
+                ),
             }
         }
 

flight/src/hardware/a4963/driver.rs

@@ -89,7 +89,7 @@ where
         self.read_diagnostic_register()
     }
 
-    fn write_configuration(&mut self) -> Result<()> {
+    fn write_configuration(&mut self, direction: Direction) -> Result<()> {
         trace!("A4963Driver::write_configuration(self: {self:?})");
 
         self.check_health()?.assert_healthy()?;
@@ -127,7 +127,7 @@ where
         self.write_verify(RunRegister {
             motor_control_mode: MotorControlMode::ClosedLoopSpeed,
             restart_after_loss_of_sync: false, // I want to handle this in my code
-            direction: Direction::Forward, // A = Black, B = Yellow, C = Red
+            direction,
             enable: true,
             ..Default::default()
         })?.assert_healthy()?;

flight/src/hardware/a4963/mod.rs

@@ -2,6 +2,8 @@ mod driver;
 mod register;
 
 use anyhow::Result;
+pub use crate::hardware::a4963::register::Direction;
+pub use driver::A4963Driver;
 
 #[derive(Clone, Debug, PartialEq, Eq)]
 pub struct A4963Diagnostic {
@@ -24,9 +26,5 @@ pub trait A4963 {
 
     fn check_health(&mut self) -> Result<A4963Diagnostic>;
 
-    fn write_configuration(&mut self) -> Result<()>;
-
-    // fn check_health(&mut self) -> Result<()>;
+    fn write_configuration(&mut self, direction: Direction) -> Result<()>;
 }
-
-pub use driver::A4963Driver;

flight/src/hardware/a4963/register.rs

@@ -419,7 +419,7 @@ pub(super) enum MotorControlMode {
 
 #[derive(Copy, Clone, Debug, FromRepr, PartialEq, Eq)]
 #[repr(u8)]
-pub(super) enum Direction {
+pub enum Direction {
     Forward = 0b0,
     Reverse = 0b1,
 }