adds the code used for the controller demo

flight/src/drive/mod.rs

@@ -0,0 +1,24 @@
+use nautilus_common::command::set_pwm::SetPwm;
+use nautilus_common::command::valid_priority_command::ValidPriorityCommand;
+use std::time::Instant;
+
+pub mod task;
+
+pub trait Drive {
+    fn set_throttle(&self, throttle: f64, valid_until: Instant, priority: u8);
+
+
+    fn new_set_throttle_callback<'a>(&self) -> impl Fn(ValidPriorityCommand<SetPwm>) + 'a
+    where
+        Self: Sized + Clone + 'a,
+    {
+        let this = self.clone();
+        move |cmd| {
+            this.set_throttle(
+                cmd.throttle,
+                cmd.get_valid_until_instant(),
+                cmd.priority,
+            );
+        }
+    }
+}

flight/src/drive/task.rs

@@ -0,0 +1,211 @@
+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 crate::drive::Drive;
+use crate::hardware::a4963::A4963;
+use crate::hardware::pwm::PwmOutput;
+use crate::scheduler::{CyclicTask, TaskHandle};
+
+const OFF_THROTTLE: f64 = 0.0;
+const THRESHOLD_THROTTLE: f64 = 0.2;
+const MINIMUM_THROTTLE: f64 = 0.3;
+const STARTUP_THROTTLE: f64 = 0.5;
+const MAXIMUM_THROTTLE: f64 = 1.0;
+
+const STARTUP_TIME: u16 = 5; // 5 cycles = 0.5 second
+
+#[derive(Clone, Debug)]
+pub enum DriveMessage {
+    SetThrust(f64),
+}
+
+impl<D: Debug> Drive for TaskHandle<DriveMessage, D> {
+    fn set_throttle(&self, throttle: f64, valid_until: Instant, priority: u8) {
+        trace!(
+            "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));
+    }
+}
+
+pub struct DriveTask<MotorController, Pwm> {
+    motor_controller: MotorController,
+    pwm: Pwm,
+    thrust_setpoint: f64,
+    state: State,
+}
+
+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: {:?} }}",
+            type_name::<MotorController>(),
+            type_name::<Pwm>(),
+            self.thrust_setpoint,
+            self.state,
+        )
+    }
+}
+
+#[derive(Clone, Debug)]
+enum State {
+    Off,
+    Configure,
+    Startup {
+        timer: u16,
+    },
+    Operational,
+    Error,
+}
+
+impl<MotorController, Pwm> DriveTask<MotorController, Pwm>
+where
+    MotorController: A4963,
+    Pwm: PwmOutput
+{
+    pub fn new(motor_controller: MotorController, pwm: Pwm) -> Self {
+        Self {
+            motor_controller,
+            pwm,
+            thrust_setpoint: OFF_THROTTLE,
+            state: State::Off,
+        }
+    }
+
+    fn step_state(&mut self, state: State) -> State {
+        trace!("DriveTask::step_state(self: {self:?}, state: {state:?})");
+        match state {
+            State::Error => {
+                // A generic error state which cleans up
+                // any internals
+                self.thrust_setpoint = OFF_THROTTLE;
+
+                State::Off
+            },
+            State::Off => {
+                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);
+                }
+
+                state
+            }
+            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);
+                    }
+                };
+                if let Err(err) = diagnostic.assert_consistency() {
+                    error!("DriveTask::step_state(self: {self:?}, state: {state:?} - {err}")
+                }
+                if diagnostic.power_on_reset {
+                    // We just had a power on or a reset event
+                    // That's perfectly Ok as long as we only see it once
+                    info!("Encountered a power on/reset");
+                    return state;
+                }
+
+                match self.motor_controller.write_configuration() {
+                    Ok(()) => State::Startup {
+                        timer: 0,
+                    },
+                    Err(err) => {
+                        error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Failed to write motor configuration: {err}");
+                        self.step_state(State::Error)
+                    },
+                }
+            }
+            State::Startup { timer } => {
+                if self.thrust_setpoint < THRESHOLD_THROTTLE {
+                    return self.step_state(State::Off);
+                }
+                if let Err(err) = self.pwm.set_duty_cycle(STARTUP_THROTTLE) {
+                    error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Pwm Error: {err}");
+                    return self.step_state(State::Error);
+                }
+
+                // remain in the current state
+                if timer < STARTUP_TIME {
+                    State::Startup {
+                        timer: timer.saturating_add(1)
+                    }
+                } else {
+                    State::Operational
+                }
+            }
+            State::Operational => {
+                if self.thrust_setpoint < THRESHOLD_THROTTLE {
+                    return self.step_state(State::Off);
+                }
+
+                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);
+                    }
+                };
+                if let Err(err) = diagnostic.assert_consistency() {
+                    error!("DriveTask::step_state(self: {self:?}, state: {state:?} - {err}")
+                }
+                if diagnostic.fault_flag {
+                    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;
+                        }
+                        warn!("Loss of sync");
+                        return State::Startup { timer: 0 };
+                    }
+                    error!("DriveTask::step_state(self: {self:?}, state: {state:?} - Diagnostic: {diagnostic:?}");
+                    return State::Error;
+                }
+
+                let new_thrust = self.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);
+                }
+
+                State::Operational
+            }
+        }
+    }
+}
+
+impl<MotorController, Pwm> CyclicTask for DriveTask<MotorController, Pwm>
+where
+    MotorController: A4963,
+    Pwm: PwmOutput
+{
+    type Message = DriveMessage;
+    type Data = ();
+
+    fn get_data(&self) -> Self::Data {
+        ()
+    }
+
+    fn step(&mut self, receiver: &Receiver<Self::Message>, step_time: Instant) {
+        trace!("DriveTask::step(self: {self:?}, receiver: {receiver:?}, step_time: {step_time:?})");
+
+        while let Ok(message) = receiver.try_recv() {
+            match message {
+                DriveMessage::SetThrust(new_thrust) => self.thrust_setpoint = new_thrust,
+            }
+        }
+
+        self.state = self.step_state(self.state.clone());
+    }
+}

flight/src/hardware/a4963/driver.rs

@@ -1,12 +1,10 @@
 use std::any::type_name;
 use std::fmt::{Debug, Formatter};
-use std::thread::sleep;
-use std::time::Duration;
-use crate::hardware::a4963::A4963;
+use crate::hardware::a4963::{A4963Diagnostic, A4963};
 use anyhow::{ensure, Result};
 use embedded_hal::spi::SpiDevice;
-use log::{trace, warn};
-use crate::hardware::a4963::register::{A4963Register, Configuration0Register, Configuration1Register, Configuration2Register, Configuration3Register, Configuration4Register, Configuration5Register, DiagnosticRegister, Direction, MaskRegister, MotorControlMode, RunRegister, SpeedOutputSelection, WRITE_BIT};
+use log::trace;
+use crate::hardware::a4963::register::{A4963Register, Configuration0Register, Configuration1Register, Configuration2Register, Configuration3Register, Configuration4Register, Configuration5Register, Direction, MaskRegister, MotorControlMode, RunRegister, SpeedOutputSelection, WRITE_BIT};
 use crate::hardware::error::WrappingError;
 
 pub struct A4963Driver<SPI> {
@@ -25,24 +23,26 @@ where
             "A4963Driver<SPI={}>::new(spi)",
             type_name::<SPI>()
         );
-        Self {
+        let mut result = Self {
             spi,
             mask_register: MaskRegister {
                 ..Default::default()
             },
-        }
+        };
+
+        let _ = result.write(RunRegister {
+            enable: false,
+            ..Default::default()
+        });
+
+        result
     }
 
-    fn update_mask_register(&mut self, mask_register: MaskRegister) -> Result<DiagnosticRegister> {
-        self.mask_register = mask_register;
-        self.write_verify(self.mask_register.clone())
-    }
-
-    fn read_diagnostic_register(&mut self) -> Result<DiagnosticRegister> {
+    fn read_diagnostic_register(&mut self) -> Result<A4963Diagnostic> {
         self.write(self.mask_register.clone())
     }
 
-    fn write<Register: A4963Register>(&mut self, register: Register) -> Result<DiagnosticRegister> {
+    fn write<Register: A4963Register>(&mut self, register: Register) -> Result<A4963Diagnostic> {
         trace!("A4963Driver::write(self: {self:?}, register: {register:?})");
         let mut rx_buffer = [0u8; 2];
         let tx_data = register.encode() | Register::ADDRESS | WRITE_BIT;
@@ -51,7 +51,7 @@ where
         self.spi.transfer(&mut rx_buffer, &tx_buffer).map_err(WrappingError)?;
         let rx_data = u16::from_be_bytes(rx_buffer);
         trace!("A4963Driver::write - rx = {rx_data:016b}");
-        Ok(DiagnosticRegister::decode(rx_data))
+        Ok(A4963Diagnostic::decode(rx_data))
     }
 
     fn read<Register: A4963Register>(&mut self) -> Result<Register> {
@@ -62,12 +62,12 @@ where
         self.spi.transfer(&mut rx_buffer, &tx_buffer).map_err(WrappingError)?;
         let rx_data = u16::from_be_bytes(rx_buffer);
         trace!("A4963Driver::read - rx {} = {rx_data:016b}", type_name::<Register>());
-        DiagnosticRegister::decode(rx_data & 0b1110_0000_0000_0000)
+        A4963Diagnostic::decode(rx_data & 0b1110_0000_0000_0000)
             .assert_healthy()?;
         Ok(Register::decode(rx_data))
     }
 
-    fn write_verify<Register: A4963Register>(&mut self, register: Register) -> Result<DiagnosticRegister> {
+    fn write_verify<Register: A4963Register>(&mut self, register: Register) -> Result<A4963Diagnostic> {
         let result = self.write(register.clone())?;
         let readback = self.read()?;
 
@@ -84,14 +84,15 @@ where
     SPI::Error: Sync,
     SPI::Error: 'static, {
 
-    fn init(&mut self) -> Result<()> {
-        trace!("A4963Driver::init(self: {self:?})");
+    fn check_health(&mut self) -> Result<A4963Diagnostic> {
+        trace!("A4963Driver::check_health(self: {self:?})");
+        self.read_diagnostic_register()
+    }
 
-        // Write the fault mask first
-        self.update_mask_register(MaskRegister {
-            enable_loss_of_synchronization: false,
-            ..Default::default()
-        })?.assert_healthy()?;
+    fn write_configuration(&mut self) -> Result<()> {
+        trace!("A4963Driver::write_configuration(self: {self:?})");
+
+        self.check_health()?.assert_healthy()?;
         // Write our configurations
         self.write_verify(Configuration0Register {
             ..Default::default()
@@ -105,33 +106,34 @@ where
             ..Default::default()
         })?.assert_healthy()?;
         self.write_verify(Configuration3Register {
+            position_integral_gain: 0b1001,
             ..Default::default()
         })?.assert_healthy()?;
         self.write_verify(Configuration4Register {
-            forced_startup_torque_duty_cycle: 0b0111,
+            forced_startup_torque_duty_cycle: 0b1111,
             start_speed: 0b1111,
             ..Default::default()
         })?.assert_healthy()?;
         self.write_verify(Configuration5Register {
-            speed_output_selection: SpeedOutputSelection::CommutationFrequency,
+            speed_integral_gain: 0b0111,
+            speed_output_selection: SpeedOutputSelection::ElectricalFrequency,
             // 819.1Hz = 49146 rpm max speed (divide by number of motor pole pairs)
             // Measured maximum of the motor is ~840Hz
             maximum_electrical_cycle_frequency: 0b101,
+            phase_advance: 0b0100,
             ..Default::default()
         })?.assert_healthy()?;
         // Set up the run mode for the motor (this enables the motor)
         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
+            enable: true,
             ..Default::default()
         })?.assert_healthy()?;
 
         Ok(())
     }
-
-    fn check_health(&mut self) -> Result<()> {
-        self.read_diagnostic_register()?.assert_healthy()
-    }
 }
 
 impl<SPI> Debug for A4963Driver<SPI> {

flight/src/hardware/a4963/mod.rs

@@ -3,10 +3,30 @@ mod register;
 
 use anyhow::Result;
 
-pub trait A4963 {
-    fn init(&mut self) -> Result<()>;
+#[derive(Clone, Debug, PartialEq, Eq)]
+pub struct A4963Diagnostic {
+    pub fault_flag: bool,
+    pub power_on_reset: bool,
+    pub serial_transfer_error: bool,
+    pub high_temperature_warning: bool,
+    pub overtemperature_shutdown: bool,
+    pub loss_of_synchronization: bool,
+    pub undervoltage: bool,
+    pub phase_a_high_side_fault: bool,
+    pub phase_a_low_side_fault: bool,
+    pub phase_b_high_side_fault: bool,
+    pub phase_b_low_side_fault: bool,
+    pub phase_c_high_side_fault: bool,
+    pub phase_c_low_side_fault: bool,
+}
 
-    fn check_health(&mut self) -> Result<()>;
+pub trait A4963 {
+
+    fn check_health(&mut self) -> Result<A4963Diagnostic>;
+
+    fn write_configuration(&mut self) -> Result<()>;
+
+    // fn check_health(&mut self) -> Result<()>;
 }
 
 pub use driver::A4963Driver;

flight/src/hardware/a4963/register.rs

@@ -1,8 +1,9 @@
 use std::fmt::Debug;
 use std::hint::unreachable_unchecked;
 use strum_macros::FromRepr;
-use anyhow::Result;
+use anyhow::{ensure, Result};
 use thiserror::Error;
+use crate::hardware::a4963::A4963Diagnostic;
 
 pub(super) const WRITE_BIT: u16 = 0b0001_0000_0000_0000;
 
@@ -485,23 +486,6 @@ impl A4963Register for RunRegister {
     }
 }
 
-#[derive(Clone, Debug, PartialEq, Eq)]
-pub(super) struct DiagnosticRegister {
-    pub fault_flag: bool,
-    pub power_on_reset: bool,
-    pub serial_transfer_error: bool,
-    pub high_temperature_warning: bool,
-    pub overtemperature_shutdown: bool,
-    pub loss_of_synchronization: bool,
-    pub undervoltage: bool,
-    pub phase_a_high_side_fault: bool,
-    pub phase_a_low_side_fault: bool,
-    pub phase_b_high_side_fault: bool,
-    pub phase_b_low_side_fault: bool,
-    pub phase_c_high_side_fault: bool,
-    pub phase_c_low_side_fault: bool,
-}
-
 #[derive(Clone, Debug, Error, PartialEq, Eq)]
 enum DiagnosticRegisterError {
     #[error("Unknown Fault")]
@@ -532,7 +516,7 @@ enum DiagnosticRegisterError {
     PhaseCLowSide,
 }
 
-impl DiagnosticRegister {
+impl A4963Diagnostic {
     pub(super) const fn decode(value: u16) -> Self {
         Self {
             fault_flag: (value & 0b1000_0000_0000_0000) != 0,
@@ -552,6 +536,25 @@ impl DiagnosticRegister {
         }
     }
 
+    pub fn assert_consistency(&self) -> Result<()> {
+        let measured_fault = self.power_on_reset ||
+            self.serial_transfer_error ||
+            self.high_temperature_warning ||
+            self.overtemperature_shutdown ||
+            self.loss_of_synchronization ||
+            self.undervoltage ||
+            self.phase_a_high_side_fault ||
+            self.phase_a_low_side_fault ||
+            self.phase_b_high_side_fault ||
+            self.phase_b_low_side_fault ||
+            self.phase_c_high_side_fault ||
+            self.phase_c_low_side_fault;
+
+        ensure!(self.fault_flag == measured_fault, "Measured Diagnostic {self:?} is Inconsistent with Fault Flag");
+
+        Ok(())
+    }
+
     pub fn assert_healthy(self) -> Result<()> {
         if self.power_on_reset {
             Err(DiagnosticRegisterError::PowerOnReset.into())

flight/src/hardware/mod.rs

@@ -1,15 +1,15 @@
 use crate::hardware::mcp23017::Mcp23017;
 use anyhow::Result;
-use embedded_hal::pwm::SetDutyCycle;
 use log::trace;
 use std::fmt::Debug;
 use crate::hardware::a4963::A4963;
+use crate::hardware::pwm::PwmOutput;
 
 pub trait Hardware {
     type Mcp23017<'a>: Mcp23017 + Send + Debug
     where
         Self: 'a;
-    type Pwm: SetDutyCycle<Error: std::error::Error + Sync + Send> + Sync + Send;
+    type Pwm: PwmOutput + Sync + Send;
 
     fn new_mcp23017_a(&self) -> Result<Self::Mcp23017<'_>>;
     fn new_mcp23017_b(&self) -> Result<Self::Mcp23017<'_>>;
@@ -52,3 +52,4 @@ mod mcp3208;
 pub mod pin;
 mod sim;
 pub mod a4963;
+pub mod pwm;

flight/src/hardware/pwm.rs

@@ -0,0 +1,6 @@
+
+use anyhow::Result;
+
+pub trait PwmOutput {
+    fn set_duty_cycle(&mut self, duty_cycle: f64) -> Result<()>;
+}

flight/src/hardware/raspi/mod.rs

@@ -13,7 +13,6 @@ use rpi_pal::spi::SimpleHalSpiDevice;
 use rpi_pal::spi::{Bus, Mode, SlaveSelect, Spi};
 use std::cell::RefCell;
 use std::sync::Mutex;
-use rpi_pal::gpio::{Gpio, InputPin};
 use crate::hardware::a4963::{A4963Driver, A4963};
 use crate::hardware::channelization::PininChannel;
 

flight/src/hardware/raspi/pwm.rs

@@ -1,8 +1,8 @@
-use crate::hardware::error::WrappingError;
-use embedded_hal::pwm::{ErrorType, SetDutyCycle};
 use log::trace;
 use rpi_pal::pwm::Pwm;
 use std::time::Duration;
+use crate::hardware::pwm::PwmOutput;
+use anyhow::Result;
 
 const PWM_PERIOD: Duration = Duration::from_micros(1000); // 1kHz
 
@@ -12,29 +12,20 @@ pub struct PwmWrapper {
 }
 
 impl PwmWrapper {
-    pub fn new(mut pwm: Pwm) -> anyhow::Result<Self> {
+    pub fn new(mut pwm: Pwm) -> Result<Self> {
         trace!("PwmWrapper::new(pwm: {pwm:?})");
         pwm.set_period(PWM_PERIOD)?;
+        pwm.set_duty_cycle(0.0)?;
         pwm.enable()?;
         pwm.set_reset_on_drop(true);
         Ok(Self { pwm })
     }
 }
 
-impl ErrorType for PwmWrapper {
-    type Error = WrappingError<rpi_pal::pwm::Error>;
-}
+impl PwmOutput for PwmWrapper {
 
-impl SetDutyCycle for PwmWrapper {
-    fn max_duty_cycle(&self) -> u16 {
-        trace!("PwmWrapper::max_duty_cycle(self: {self:?})");
-        u16::MAX
-    }
-
-    fn set_duty_cycle(&mut self, duty: u16) -> Result<(), Self::Error> {
-        trace!("PwmWrapper::set_duty_cycle(self: {self:?}, duty: {duty})");
-        self.pwm
-            .set_duty_cycle((duty as f64) / (u16::MAX as f64))
-            .map_err(WrappingError)
+    fn set_duty_cycle(&mut self, duty_cycle: f64) -> Result<()> {
+        trace!("PwmWrapper::set_duty_cycle(self: {self:?}, duty_cycle: {duty_cycle})");
+        Ok(self.pwm.set_duty_cycle(duty_cycle)?)
     }
 }

flight/src/lib.rs

@@ -1,6 +1,5 @@
 #![warn(clippy::all, clippy::pedantic)]
 
-use std::f64::consts::{PI, TAU};
 use crate::comms::{CommsState, CommsTask};
 use crate::hardware::Hardware;
 use crate::hardware::initialize;
@@ -10,8 +9,7 @@ use crate::rcs::RcsTask;
 use crate::scheduler::Scheduler;
 use crate::state_vector::StateVector;
 use anyhow::Result;
-use embedded_hal::pwm::{ErrorType, SetDutyCycle};
-use log::{error, info};
+use log::info;
 use nautilus_common::add_ctrlc_handler_arc;
 use nautilus_common::telemetry::{SwitchBank, TelemetryMessage};
 use std::sync::Arc;
@@ -19,11 +17,21 @@ use std::sync::atomic::{AtomicBool, Ordering};
 use std::thread::sleep;
 use std::time::Duration;
 use crc::Algorithm;
-use nautilus_common::command::set_pwm::SetPwm;
-use crate::hardware::a4963::A4963;
+use crate::drive::Drive;
+use crate::drive::task::DriveTask;
 
 mod hardware;
 
+mod commanded_state;
+mod comms;
+mod data;
+mod rcs;
+mod scheduler;
+mod state_vector;
+mod drive;
+#[cfg(test)]
+mod test_utils;
+
 pub const CRC_8_CCITT: Algorithm<u8> = Algorithm { width: 8, poly: 0x07, init: 0xFF, refin: false, refout: false, xorout: 0x00, check: 0xfb, residue: 0xFF };
 
 fn new_shutdown_handler(running: &Arc<AtomicBool>) -> impl Fn(()) {
@@ -48,20 +56,15 @@ pub fn run() -> Result<()> {
 
     let mut hal = initialize()?;
 
-    let mut a4963 = hal.new_a4963()?;
+    let a4963 = hal.new_a4963()?;
     let mut mcp23017_a = hal.new_mcp23017_a()?;
     let mut mcp23017_b = hal.new_mcp23017_b()?;
-    let mut pwm0 = hal.new_pwm0()?;
+    let pwm0 = hal.new_pwm0()?;
 
     info!("Battery Voltage: {}", hal.get_battery_voltage()?);
 
-    pwm0.set_duty_cycle_percent(0)?;
-
     mcp23017_a.init()?;
     mcp23017_b.init()?;
-    a4963.init()?;
-    
-    pwm0.set_duty_cycle_percent(100)?;
 
     Scheduler::scope(running.clone(), |s| {
         let task_a = s.run_cyclic(
@@ -80,19 +83,18 @@ pub fn run() -> Result<()> {
 
         let rcs = s.run_cyclic("rcs-task", RcsTask::new(&task_a, &task_b), 10)?;
 
+        let drive = s.run_cyclic(
+            "drive-task",
+            DriveTask::new(a4963, pwm0),
+            10
+        )?;
+
         let mut comms = CommsTask::new(15000, "nautilus-ground:14000", &state_vector)?;
         comms.add_command_handler("/shutdown", new_shutdown_handler(&running))?;
         comms.add_command_handler("/mcp23017a/set", task_a.new_set_pin_callback())?;
         comms.add_command_handler("/mcp23017b/set", task_b.new_set_pin_callback())?;
         comms.add_command_handler("/rcs/set", rcs.new_set_rcs_callback())?;
-        comms.add_command_handler("/pwm/set", move |pwm: SetPwm| {
-            match pwm0.set_duty_cycle_percent(pwm.duty_cycle) {
-                Ok(_) => {}
-                Err(err) => {
-                    error!("Failed to set PWM duty cycle: {err}");
-                }
-            }
-        })?;
+        comms.add_command_handler("/drive/throttle/set", drive.new_set_throttle_callback())?;
         let comms = s.run_cyclic("comms-task", comms, 10)?;
         let comms_id = *comms;
 
@@ -122,8 +124,7 @@ pub fn run() -> Result<()> {
         info!("Starting Main Loop");
 
         while running.load(Ordering::Relaxed) {
-            sleep(Duration::from_millis(100));
-            a4963.check_health()?;
+            sleep(Duration::from_millis(1000));
         }
 
         anyhow::Ok(())
@@ -142,12 +143,3 @@ pub fn run() -> Result<()> {
 
     Ok(())
 }
-
-mod commanded_state;
-mod comms;
-mod data;
-mod rcs;
-mod scheduler;
-mod state_vector;
-#[cfg(test)]
-mod test_utils;