Preliminary integration of VESC motors into FSM

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<Uart>,
+	#[cfg(feature = "vesc")]
+	pub vesc2: VescConnection<Uart>,
 }
 
 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"))]

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<dyn std::error::Error>> {
 
 	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;

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<std::sync::Mutex<WheelEncoder>>,
 	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,24 +228,28 @@ 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();
 	}
 
 	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();
 	}