From d758f9dfd1bb736a6e4acae00a7085cedfe08505 Mon Sep 17 00:00:00 2001 From: Sergey Savelyev Date: Wed, 26 Aug 2026 19:11:15 -0700 Subject: [PATCH] adds the code used for the water demo --- flight/src/drive/task.rs | 146 ++++++++++++++++++-------- flight/src/hardware/a4963/driver.rs | 4 +- flight/src/hardware/a4963/mod.rs | 8 +- flight/src/hardware/a4963/register.rs | 2 +- 4 files changed, 110 insertions(+), 50 deletions(-) diff --git a/flight/src/drive/task.rs b/flight/src/drive/task.rs index 7361f75..ec058c6 100644 --- a/flight/src/drive/task.rs +++ b/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 Drive for TaskHandle { @@ -27,14 +34,18 @@ impl Drive for TaskHandle { "TaskHandle::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 { motor_controller: MotorController, pwm: Pwm, - thrust_setpoint: f64, + thrust_setpoint: CommandedState, state: State, } @@ -42,7 +53,7 @@ impl Debug for DriveTask { fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result { write!( f, - "DriveTask {{ thrust_setpoint: {}, state: {:?} }}", + "DriveTask {{ thrust_setpoint: {:?}, state: {:?} }}", type_name::(), type_name::(), self.thrust_setpoint, @@ -53,13 +64,21 @@ impl Debug for DriveTask { #[derive(Clone, Debug)] enum State { - Off, + Off { + timer: u16, + from_direction: Option, + }, Configure, Startup { timer: u16, + direction: Direction, + }, + Operational { + direction: Direction, + }, + Error { + timer: u16, }, - Operational, - Error, } impl DriveTask @@ -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, 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, + ), } } diff --git a/flight/src/hardware/a4963/driver.rs b/flight/src/hardware/a4963/driver.rs index 1c7d5d4..85713ba 100644 --- a/flight/src/hardware/a4963/driver.rs +++ b/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()?; diff --git a/flight/src/hardware/a4963/mod.rs b/flight/src/hardware/a4963/mod.rs index dd23011..05c75b1 100644 --- a/flight/src/hardware/a4963/mod.rs +++ b/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; - fn write_configuration(&mut self) -> Result<()>; - - // fn check_health(&mut self) -> Result<()>; + fn write_configuration(&mut self, direction: Direction) -> Result<()>; } - -pub use driver::A4963Driver; diff --git a/flight/src/hardware/a4963/register.rs b/flight/src/hardware/a4963/register.rs index 6c1b1b7..e9cb77f 100644 --- a/flight/src/hardware/a4963/register.rs +++ b/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, }