diff --git a/pod-operation/src/components/motors.rs b/pod-operation/src/components/motors.rs index c727594..4382345 100644 --- a/pod-operation/src/components/motors.rs +++ b/pod-operation/src/components/motors.rs @@ -22,29 +22,47 @@ const MPH_TO_RPM: f32 = MPH_TO_IN_PER_MIN / WHEEL_DIAMETER; pub struct Motors { #[cfg(feature = "vesc")] pub vesc: VescConnection, + #[cfg(feature = "vesc")] + pub vesc2: VescConnection, } impl Motors { #[cfg(feature = "vesc")] - pub fn new(serial_path: &str) -> Self { + pub fn new(serial_path_one: &str, serial_path_two: &str) -> Self { use serial_constants::*; - let uart = Uart::with_path(serial_path, BAUD_RATE, PARITY, DATA_BITS, STOP_BITS).unwrap(); + use vesc_comm::VescConnection; + let uart = + Uart::with_path(serial_path_one, BAUD_RATE, PARITY, DATA_BITS, STOP_BITS).unwrap(); + let uart2 = + Uart::with_path(serial_path_two, BAUD_RATE, PARITY, DATA_BITS, STOP_BITS).unwrap(); let conn = VescConnection::new(uart); - info!("Initialized VESC on {}", serial_path); - Self { vesc: conn } + let conn2 = VescConnection::new(uart2); + info!( + "Initialized VESC on paths {} and {}", + serial_path_one, serial_path_two + ); + Self { + vesc: conn, + vesc2: conn2, + } } #[cfg(not(feature = "vesc"))] - pub fn new(serial_path: &str) -> Self { - info!("Mocking VESC on {}", serial_path); + pub fn new(serial_path_one: &str, serial_path_two: &str) -> Self { + info!( + "Mocking VESC on {} and {}", + serial_path_one, serial_path_two + ); Self {} } #[cfg(feature = "vesc")] pub fn set_speed_mph(&mut self, new_speed_mph: f32) -> Result<(), VescErrorWithBacktrace> { debug!("Driving motors at {}", new_speed_mph); - self.vesc - .set_rpm((new_speed_mph * MPH_TO_RPM).round() as u32) + let new_speed: u32 = (new_speed_mph * MPH_TO_RPM).round() as u32; + self.vesc.set_rpm(new_speed)?; + self.vesc2.set_rpm(new_speed)?; + Ok(()) } #[cfg(not(feature = "vesc"))] diff --git a/pod-operation/src/main.rs b/pod-operation/src/main.rs index 8c06f9a..534298a 100644 --- a/pod-operation/src/main.rs +++ b/pod-operation/src/main.rs @@ -8,17 +8,6 @@ mod demo; mod state_machine; mod utils; -use crate::components::brakes::Brakes; -use crate::components::gyro::Gyroscope; -use crate::components::high_voltage_system::HighVoltageSystem; -use crate::components::inverter_board::InverterBoard; -use crate::components::lidar::Lidar; -use crate::components::lim_current::LimCurrent; -use crate::components::lim_temperature::LimTemperature; -use crate::components::motors::Motors; -use crate::components::pressure_transducer::PressureTransducer; -use crate::components::signal_light::SignalLight; -use crate::components::wheel_encoder::WheelEncoder; use crate::state_machine::StateMachine; #[tokio::main] @@ -30,43 +19,6 @@ async fn main() -> Result<(), Box> { let (layer, io) = SocketIo::new_layer(); - let signal_light = SignalLight::new(); - tokio::spawn(demo::blink(signal_light)); - - let upstream_pressure_transducer = PressureTransducer::upstream(); - tokio::spawn(demo::read_pressure_transducer(upstream_pressure_transducer)); - - let downstream_pressure_transducer = PressureTransducer::downstream(); - tokio::spawn(demo::read_pressure_transducer( - downstream_pressure_transducer, - )); - - let ads1015 = LimTemperature::new(ads1x1x::SlaveAddr::Default); - tokio::spawn(demo::read_ads1015(ads1015)); - - let wheel_encoder = WheelEncoder::new(); - tokio::spawn(demo::read_wheel_encoder(wheel_encoder)); - - let gyro: Gyroscope = Gyroscope::new(); - tokio::spawn(demo::read_gyroscope(gyro)); - let brakes = Brakes::new(); - tokio::spawn(demo::brake(brakes)); - - let high_voltage_system = HighVoltageSystem::new(); - tokio::spawn(demo::high_voltage_system(high_voltage_system)); - - let lidar = Lidar::new(); - tokio::spawn(demo::read_lidar(lidar)); - - let limcurrent = LimCurrent::new(ads1x1x::SlaveAddr::Default); - tokio::spawn(demo::read_lim_current(limcurrent)); - - let inverter_board = InverterBoard::new(); - tokio::spawn(demo::inverter_control(inverter_board)); - - let motors = Motors::new("/dev/ttyACM0"); - tokio::spawn(demo::vesc_motors(motors)); - let mut state_machine = StateMachine::new(io); tokio::spawn(async move { state_machine.run().await; diff --git a/pod-operation/src/state_machine.rs b/pod-operation/src/state_machine.rs index 34d1317..5513628 100644 --- a/pod-operation/src/state_machine.rs +++ b/pod-operation/src/state_machine.rs @@ -14,6 +14,7 @@ use crate::components::gyro::Gyroscope; use crate::components::high_voltage_system::HighVoltageSystem; use crate::components::lidar::Lidar; use crate::components::lim_temperature::LimTemperature; +use crate::components::motors::Motors; use crate::components::pressure_transducer::PressureTransducer; use crate::components::signal_light::SignalLight; use crate::components::wheel_encoder::WheelEncoder; @@ -55,6 +56,7 @@ pub struct StateMachine { lidar: Lidar, wheel_encoder: std::sync::Arc>, gyro: Gyroscope, + motors: Motors, } impl StateMachine { @@ -113,6 +115,7 @@ impl StateMachine { lidar: Lidar::new(), gyro: Gyroscope::new(), wheel_encoder: std::sync::Arc::new(std::sync::Mutex::new(WheelEncoder::new())), + motors: Motors::new("/dev/ttyACM0", "/dev/ttyACM1"), } } @@ -225,17 +228,20 @@ impl StateMachine { self.high_voltage_system.enable(); // Enable high voltage system -- may move later self.signal_light.enable(); self.brakes.disengage(); + self.motors.set_speed_mph(10.0).unwrap(); } fn _enter_stopped(&mut self) { info!("Entering Stopped state"); self.signal_light.disable(); + self.motors.set_speed_mph(0.0).unwrap(); self.brakes.engage(); } fn _enter_halted(&mut self) { info!("Entering Halted state"); self.signal_light.disable(); + self.motors.set_speed_mph(0.0).unwrap(); self.brakes.engage(); self.high_voltage_system.disable(); } @@ -243,6 +249,7 @@ impl StateMachine { fn _enter_faulted(&mut self) { info!("Entering Faulted state"); self.signal_light.disable(); + self.motors.set_speed_mph(0.0).unwrap(); self.brakes.engage(); self.high_voltage_system.disable(); }