Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Preliminary integration of VESC motors into FSM #123

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Preliminary integration of VESC motors into FSM #123

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 26 additions & 8 deletions pod-operation/src/components/motors.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"))]
Expand Down
48 changes: 0 additions & 48 deletions pod-operation/src/main.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -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;
Expand Down
7 changes: 7 additions & 0 deletions pod-operation/src/state_machine.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -55,6 +56,7 @@ pub struct StateMachine {
lidar: Lidar,
wheel_encoder: std::sync::Arc<std::sync::Mutex<WheelEncoder>>,
gyro: Gyroscope,
motors: Motors,
}

impl StateMachine {
Expand Down Expand Up @@ -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"),
}
}

Expand Down Expand Up @@ -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();
}
Expand Down
Loading