From 1e316148d85f7d8c63f7bb39b41cf32ce3a74f57 Mon Sep 17 00:00:00 2001 From: Benjamin Bolte Date: Mon, 7 Oct 2024 20:34:15 -0700 Subject: [PATCH] stuff --- actuator/cli.py | 13 +- actuator/rust/py/Cargo.toml | 4 +- actuator/rust/py/src/lib.rs | 2 +- actuator/rust/robstride/Cargo.toml | 1 + actuator/rust/robstride/src/lib.rs | 289 ++++++++++++++++++++++------ actuator/rust/robstride/src/main.rs | 20 +- setup.py | 4 +- 7 files changed, 245 insertions(+), 88 deletions(-) diff --git a/actuator/cli.py b/actuator/cli.py index e24225a..a9d0f31 100644 --- a/actuator/cli.py +++ b/actuator/cli.py @@ -1,19 +1,10 @@ """Defines the CLI for the actuator project.""" -import time - -from actuator.rust.lib import Actuator, ActuatorType +from actuator.rust.py import hello_world def main() -> None: - act_1 = Actuator(ActuatorType.Robstride, True) - act_1.set_position(0, 0.5) - act_2 = Actuator(ActuatorType.Robstride, True) - act_2.set_position(1, 0.5) - act_2.set_pid_params(1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1) - time.sleep(5) - print(act_1) - print(act_2) + hello_world() if __name__ == "__main__": diff --git a/actuator/rust/py/Cargo.toml b/actuator/rust/py/Cargo.toml index a224e26..d9ab01c 100644 --- a/actuator/rust/py/Cargo.toml +++ b/actuator/rust/py/Cargo.toml @@ -1,5 +1,5 @@ [package] -name = "lib" +name = "py" version.workspace = true edition.workspace = true description.workspace = true @@ -9,7 +9,7 @@ license.workspace = true readme.workspace = true [lib] -name = "lib" +name = "py" crate-type = ["cdylib", "rlib"] [dependencies] diff --git a/actuator/rust/py/src/lib.rs b/actuator/rust/py/src/lib.rs index bcbbdec..f1f5a65 100644 --- a/actuator/rust/py/src/lib.rs +++ b/actuator/rust/py/src/lib.rs @@ -8,7 +8,7 @@ fn hello_world() -> PyResult<()> { } #[pymodule] -fn lib(m: &Bound) -> PyResult<()> { +fn py(m: &Bound) -> PyResult<()> { m.add_function(wrap_pyfunction!(hello_world, m)?)?; Ok(()) } diff --git a/actuator/rust/robstride/Cargo.toml b/actuator/rust/robstride/Cargo.toml index a45b232..df41630 100644 --- a/actuator/rust/robstride/Cargo.toml +++ b/actuator/rust/robstride/Cargo.toml @@ -14,3 +14,4 @@ crate-type = ["cdylib", "rlib"] [dependencies] serialport = "4.5.1" +lazy_static = "1.4.0" diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index 6228d71..d36b8e9 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -1,7 +1,11 @@ use serialport::SerialPort; +use std::collections::HashMap; use std::io::{Read, Write}; use std::time::Duration; +#[macro_use] +extern crate lazy_static; + pub const CAN_ID_MASTER: u8 = 0x00; pub const CAN_ID_MOTOR_DEFAULT: u8 = 0x7F; pub const CAN_ID_BROADCAST: u8 = 0xFE; @@ -22,44 +26,57 @@ pub struct MotorConfig { pub t_max: f32, } -pub const ROBSTRIDE01_CONFIG: MotorConfig = MotorConfig { - p_min: -12.5, - p_max: 12.5, - v_min: -44.0, - v_max: 44.0, - kp_min: 0.0, - kp_max: 500.0, - kd_min: 0.0, - kd_max: 5.0, - t_min: -12.0, - t_max: 12.0, -}; - -pub const ROBSTRIDE03_CONFIG: MotorConfig = MotorConfig { - p_min: -12.5, - p_max: 12.5, - v_min: -20.0, - v_max: 20.0, - kp_min: 0.0, - kp_max: 5000.0, - kd_min: 0.0, - kd_max: 100.0, - t_min: -60.0, - t_max: 60.0, -}; - -pub const ROBSTRIDE04_CONFIG: MotorConfig = MotorConfig { - p_min: -12.5, - p_max: 12.5, - v_min: -15.0, - v_max: 15.0, - kp_min: 0.0, - kp_max: 5000.0, - kd_min: 0.0, - kd_max: 100.0, - t_min: -120.0, - t_max: 120.0, -}; +lazy_static! { + pub static ref ROBSTRIDE_CONFIGS: HashMap<&'static str, MotorConfig> = { + let mut m = HashMap::new(); + m.insert( + "01", + MotorConfig { + p_min: -12.5, + p_max: 12.5, + v_min: -44.0, + v_max: 44.0, + kp_min: 0.0, + kp_max: 500.0, + kd_min: 0.0, + kd_max: 5.0, + t_min: -12.0, + t_max: 12.0, + }, + ); + m.insert( + "03", + MotorConfig { + p_min: -12.5, + p_max: 12.5, + v_min: -20.0, + v_max: 20.0, + kp_min: 0.0, + kp_max: 5000.0, + kd_min: 0.0, + kd_max: 100.0, + t_min: -60.0, + t_max: 60.0, + }, + ); + m.insert( + "04", + MotorConfig { + p_min: -12.5, + p_max: 12.5, + v_min: -15.0, + v_max: 15.0, + kp_min: 0.0, + kp_max: 5000.0, + kd_min: 0.0, + kd_max: 100.0, + t_min: -120.0, + t_max: 120.0, + }, + ); + m + }; +} #[repr(u8)] #[derive(Copy, Clone)] @@ -98,13 +115,15 @@ pub enum MotorMode { Brake, } +#[derive(Copy, Clone, PartialEq)] pub enum RunMode { + UnsetMode = -1, MitMode = 0, - PositionMode, - SpeedMode, - CurrentMode, - ToZeroMode, - CspPositionMode, + PositionMode = 1, + SpeedMode = 2, + CurrentMode = 3, + ToZeroMode = 4, + CspPositionMode = 5, } pub struct ExId { @@ -247,23 +266,41 @@ fn read_bytes( } } -pub struct Motor { +struct Motor { port: Box, - config: MotorConfig, + config: &'static MotorConfig, id: u8, + current_mode: RunMode, + pending_responses: usize, } impl Motor { pub fn new( tty_port: &str, - config: MotorConfig, + config: &'static MotorConfig, id: u8, ) -> Result> { let port = init_serial_port(tty_port)?; - Ok(Motor { port, config, id }) + Ok(Motor { + port, + config, + id, + current_mode: RunMode::UnsetMode, + pending_responses: 0, + }) } - pub fn send_set_mode(&mut self, run_mode: RunMode) -> Result { + fn send_command(&mut self, pack: &CanPack) -> Result<(), std::io::Error> { + txd_pack(&mut self.port, pack)?; + self.pending_responses += 1; + Ok(()) + } + + fn send_set_mode(&mut self, run_mode: RunMode) -> Result<(), std::io::Error> { + if self.current_mode == run_mode { + return Ok(()); + } + let mut pack = CanPack { ex_id: ExId { id: self.id, @@ -279,11 +316,12 @@ impl Motor { pack.data[..2].copy_from_slice(&index.to_le_bytes()); pack.data[4] = run_mode as u8; - txd_pack(&mut self.port, &pack)?; - read_bytes(&mut self.port, &self.config) + self.send_command(&pack)?; + self.current_mode = run_mode; + Ok(()) } - pub fn send_reset(&mut self) -> Result { + fn send_reset(&mut self) -> Result<(), std::io::Error> { let pack = CanPack { ex_id: ExId { id: self.id, @@ -295,11 +333,10 @@ impl Motor { data: [0; 8], }; - txd_pack(&mut self.port, &pack)?; - read_bytes(&mut self.port, &self.config) + self.send_command(&pack) } - pub fn send_start(&mut self) -> Result { + fn send_start(&mut self) -> Result<(), std::io::Error> { let pack = CanPack { ex_id: ExId { id: self.id, @@ -311,11 +348,10 @@ impl Motor { data: [0; 8], }; - txd_pack(&mut self.port, &pack)?; - read_bytes(&mut self.port, &self.config) + self.send_command(&pack) } - pub fn send_set_speed_limit(&mut self, speed: f32) -> Result { + fn send_set_speed_limit(&mut self, speed: f32) -> Result<(), std::io::Error> { let mut pack = CanPack { ex_id: ExId { id: self.id, @@ -331,11 +367,12 @@ impl Motor { pack.data[..2].copy_from_slice(&index.to_le_bytes()); pack.data[4..8].copy_from_slice(&speed.to_le_bytes()); - txd_pack(&mut self.port, &pack)?; - read_bytes(&mut self.port, &self.config) + self.send_command(&pack) } - pub fn send_set_location(&mut self, location: f32) -> Result { + fn send_set_location(&mut self, location: f32) -> Result<(), std::io::Error> { + self.send_set_mode(RunMode::PositionMode)?; + let mut pack = CanPack { ex_id: ExId { id: self.id, @@ -351,7 +388,135 @@ impl Motor { pack.data[..2].copy_from_slice(&index.to_le_bytes()); pack.data[4..8].copy_from_slice(&location.to_le_bytes()); - txd_pack(&mut self.port, &pack)?; - read_bytes(&mut self.port, &self.config) + self.send_command(&pack) + } + + fn send_motor_control( + &mut self, + pos_set: f32, + vel_set: f32, + kp_set: f32, + kd_set: f32, + torque_set: f32, + ) -> Result<(), std::io::Error> { + self.send_set_mode(RunMode::MitMode)?; + + let mut pack = CanPack { + ex_id: ExId { + id: self.id, + data: 0, + mode: CanComMode::MotorCtrl, + res: 0, + }, + len: 8, + data: [0; 8], + }; + + let pos_int_set = float_to_uint(pos_set, self.config.p_min, self.config.p_max, 16); + let vel_int_set = float_to_uint(vel_set, self.config.v_min, self.config.v_max, 16); + let kp_int_set = float_to_uint(kp_set, self.config.kp_min, self.config.kp_max, 16); + let kd_int_set = float_to_uint(kd_set, self.config.kd_min, self.config.kd_max, 16); + let torque_int_set = float_to_uint(torque_set, self.config.t_min, self.config.t_max, 16); + + pack.ex_id.data = torque_int_set; + + pack.data[0] = (pos_int_set >> 8) as u8; + pack.data[1] = (pos_int_set & 0xFF) as u8; + pack.data[2] = (vel_int_set >> 8) as u8; + pack.data[3] = (vel_int_set & 0xFF) as u8; + pack.data[4] = (kp_int_set >> 8) as u8; + pack.data[5] = (kp_int_set & 0xFF) as u8; + pack.data[6] = (kd_int_set >> 8) as u8; + pack.data[7] = (kd_int_set & 0xFF) as u8; + + self.send_command(&pack) + } + + fn send_position_control(&mut self, pos_set: f32, kp_set: f32) -> Result<(), std::io::Error> { + self.send_motor_control(pos_set, 0.0, kp_set, 0.0, 0.0) + } + + fn send_torque_control(&mut self, torque_set: f32) -> Result<(), std::io::Error> { + self.send_motor_control(0.0, 0.0, 0.0, 0.0, torque_set) + } + + fn read_all_pending_responses(&mut self) -> Result, std::io::Error> { + let mut feedbacks = Vec::new(); + while self.pending_responses > 0 { + match read_bytes(&mut self.port, self.config) { + Ok(feedback) => { + feedbacks.push(feedback); + self.pending_responses -= 1; + } + Err(e) => { + // If there's an error, we'll still decrement pending_responses + // to avoid getting stuck in an infinite loop + self.pending_responses -= 1; + eprintln!("Error reading response: {:?}", e); + } + } + } + Ok(feedbacks) + } +} + +// Add this helper function outside of the Motor impl block +fn float_to_uint(x: f32, x_min: f32, x_max: f32, bits: u8) -> u16 { + let span = x_max - x_min; + let offset = x_min; + ((x - offset) * ((1 << bits) - 1) as f32 / span) as u16 +} + +pub struct Motors { + motors: Vec, +} + +impl Motors { + pub fn new( + tty_port: &str, + motor_configs: &[(&'static MotorConfig, u8)], + ) -> Result> { + let motors = motor_configs + .iter() + .map(|(config, id)| Motor::new(tty_port, config, *id)) + .collect::, Box>>()?; + + Ok(Motors { motors }) + } + + pub fn send_set_locations( + &mut self, + locations: &HashMap, + ) -> Result<(), std::io::Error> { + for motor in &mut self.motors { + if let Some(&location) = locations.get(&motor.id) { + motor.send_set_location(location)?; + } + } + Ok(()) + } + + pub fn send_set_speed_limits( + &mut self, + speed_limits: &HashMap, + ) -> Result<(), std::io::Error> { + for motor in &mut self.motors { + if let Some(&speed_limit) = speed_limits.get(&motor.id) { + motor.send_set_speed_limit(speed_limit)?; + } + } + Ok(()) + } + + pub fn read_all_pending_responses( + &mut self, + ) -> Result, std::io::Error> { + let mut feedbacks = HashMap::new(); + for motor in &mut self.motors { + for feedback in motor.read_all_pending_responses()? { + feedbacks.insert(feedback.can_id, feedback); + } + } + Ok(feedbacks) } } diff --git a/actuator/rust/robstride/src/main.rs b/actuator/rust/robstride/src/main.rs index caede6a..49de3f7 100644 --- a/actuator/rust/robstride/src/main.rs +++ b/actuator/rust/robstride/src/main.rs @@ -1,32 +1,32 @@ -use robstride::{Motor, RunMode, ROBSTRIDE01_CONFIG}; +use robstride::{Motors, RunMode, ROBSTRIDE_CONFIGS}; use std::thread; use std::time::Duration; fn main() -> Result<(), Box> { println!("Starting program"); - let mut motor = Motor::new("/dev/ttyUSB0", ROBSTRIDE01_CONFIG, 2)?; + let motors = Motors::new("/dev/ttyUSB0", &[(&ROBSTRIDE_CONFIGS["01"], 2)])?; - motor.send_set_mode(RunMode::PositionMode)?; + motors.send_set_mode(RunMode::PositionMode)?; thread::sleep(Duration::from_millis(50)); - motor.send_reset()?; - motor.send_start()?; + motors.send_reset()?; + motors.send_start()?; thread::sleep(Duration::from_millis(50)); - motor.send_set_speed_limit(5.0)?; + motors.send_set_speed_limit(5.0)?; thread::sleep(Duration::from_millis(50)); for i in 0..3 { - motor.send_set_location(std::f32::consts::PI * i as f32 / 2.0)?; + motors.send_set_location(std::f32::consts::PI * i as f32 / 2.0)?; thread::sleep(Duration::from_secs(1)); } - motor.send_set_speed_limit(10.0)?; - motor.send_set_location(0.0)?; + motors.send_set_speed_limit(10.0)?; + motors.send_set_location(0.0)?; thread::sleep(Duration::from_secs(2)); - motor.send_reset()?; + motors.send_reset()?; println!("Program finished"); Ok(()) diff --git a/setup.py b/setup.py index 1f492df..bf83bfe 100644 --- a/setup.py +++ b/setup.py @@ -33,8 +33,8 @@ url="https://github.com/kscalelabs/actuator", rust_extensions=[ RustExtension( - target="actuator.rust.lib", - path="actuator/rust/Cargo.toml", + target="actuator.rust.py", + path="actuator/rust/py/Cargo.toml", binding=Binding.PyO3, ), ],