From 77a94f34eff24e987e3e9b6abe792fe6943f7de0 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Sun, 3 Nov 2024 01:49:35 -0400 Subject: [PATCH] torque limit draft --- actuator/bindings/src/lib.rs | 4 +- actuator/robstride/src/bin/multisupervisor.rs | 3 ++ actuator/robstride/src/bin/supervisor.rs | 3 ++ actuator/robstride/src/lib.rs | 51 ++++++++++++++++++- 4 files changed, 59 insertions(+), 2 deletions(-) diff --git a/actuator/bindings/src/lib.rs b/actuator/bindings/src/lib.rs index 038bcf7..f156d2f 100644 --- a/actuator/bindings/src/lib.rs +++ b/actuator/bindings/src/lib.rs @@ -230,13 +230,14 @@ struct PyRobstrideMotorsSupervisor { #[pymethods] impl PyRobstrideMotorsSupervisor { #[new] - #[pyo3(signature = (port_name, motor_infos, verbose = false, target_update_rate = 50.0, zero_on_init = false))] + #[pyo3(signature = (port_name, motor_infos, verbose = false, target_update_rate = 50.0, zero_on_init = false, safe_mode = false))] fn new( port_name: String, motor_infos: HashMap, verbose: bool, target_update_rate: f64, zero_on_init: bool, + safe_mode: bool, ) -> PyResult { let motor_infos = motor_infos .into_iter() @@ -252,6 +253,7 @@ impl PyRobstrideMotorsSupervisor { verbose, target_update_rate, zero_on_init, + safe_mode, ) .map_err(|e| PyErr::new::(e.to_string()))?; diff --git a/actuator/robstride/src/bin/multisupervisor.rs b/actuator/robstride/src/bin/multisupervisor.rs index 8a9fd2d..92607cf 100644 --- a/actuator/robstride/src/bin/multisupervisor.rs +++ b/actuator/robstride/src/bin/multisupervisor.rs @@ -16,6 +16,8 @@ struct Args { max_update_rate: f64, #[arg(long, help = "Zero on init", default_value_t = false)] zero_on_init: bool, + #[arg(long, help = "Safe mode", default_value_t = false)] + safe_mode: bool, } fn sinusoid( @@ -131,6 +133,7 @@ fn main() -> Result<(), Box> { args.verbose, args.max_update_rate, args.zero_on_init, + args.safe_mode, )?; println!("Motor Controller Test CLI"); diff --git a/actuator/robstride/src/bin/supervisor.rs b/actuator/robstride/src/bin/supervisor.rs index 96686ea..5b2c11f 100644 --- a/actuator/robstride/src/bin/supervisor.rs +++ b/actuator/robstride/src/bin/supervisor.rs @@ -16,6 +16,8 @@ struct Args { max_update_rate: f64, #[arg(long, help = "Zero on init", default_value_t = false)] zero_on_init: bool, + #[arg(long, help = "Safe mode", default_value_t = false)] + safe_mode: bool, } fn sinusoid( @@ -93,6 +95,7 @@ fn main() -> Result<(), Box> { args.verbose, args.max_update_rate, args.zero_on_init, + args.safe_mode, )?; println!("Motor Controller Test CLI"); diff --git a/actuator/robstride/src/lib.rs b/actuator/robstride/src/lib.rs index 217ea01..29584b2 100644 --- a/actuator/robstride/src/lib.rs +++ b/actuator/robstride/src/lib.rs @@ -439,6 +439,11 @@ pub fn motor_type_from_str(s: &str) -> Result { } } +#[derive(Debug, Clone, Copy, Serialize, Deserialize)] +pub struct MotorSdoParams { + pub torque_limit: f32, +} + #[derive(Debug, Clone, Copy, Serialize, Deserialize)] pub struct MotorControlParams { pub position: f32, @@ -682,6 +687,31 @@ impl Motors { Ok(()) } + fn set_torque_limit(&mut self, motor_id: u8, torque_limit: f32) -> Result<(), std::io::Error> { + let config = *self.motor_configs.get(&motor_id).ok_or(std::io::Error::new( + std::io::ErrorKind::NotFound, + "Motor not found", + ))?; + + let mut pack = CanPack { + ex_id: ExId { + id: motor_id, + data: CAN_ID_DEBUG_UI as u16, + mode: CanComMode::SdoWrite, + res: 0, + }, + len: 8, + data: vec![0; 8], + }; + let index: u16 = 0x700b; + pack.data[..2].copy_from_slice(&index.to_le_bytes()); + + let torque_limit_int = float_to_uint(torque_limit, config.t_min, config.t_max, 16); + pack.data[4..8].copy_from_slice(&torque_limit_int.to_le_bytes()); + self.send_command(&pack, true)?; + Ok(()) + } + fn read_u16_param(&mut self, motor_id: u8, index: u16) -> Result { let mut pack = CanPack { ex_id: ExId { @@ -924,6 +954,7 @@ pub struct MotorsSupervisor { running: Arc>, latest_feedback: Arc>>, motors_to_zero: Arc>>, + motors_to_set_sdo: Arc>>, paused: Arc>, restart: Arc>, total_commands: Arc>, @@ -975,13 +1006,14 @@ impl MotorsSupervisor { let motor_ids: Vec = motor_infos.keys().cloned().collect(); let total_commands = 0; let failed_commands = motor_ids.iter().map(|&id| (id, 0)).collect(); -g + let controller = MotorsSupervisor { motors: Arc::new(Mutex::new(motors)), target_params: Arc::new(RwLock::new(target_params)), running: Arc::new(RwLock::new(true)), latest_feedback: Arc::new(RwLock::new(HashMap::new())), motors_to_zero: Arc::new(Mutex::new(zero_on_init_motors)), + motors_to_set_sdo: Arc::new(Mutex::new(HashMap::new())), paused: Arc::new(RwLock::new(false)), restart: Arc::new(Mutex::new(false)), total_commands: Arc::new(RwLock::new(total_commands)), @@ -1011,6 +1043,7 @@ g let actual_update_rate = Arc::clone(&self.actual_update_rate); let serial = Arc::clone(&self.serial); let safe_mode = Arc::clone(&self.safe_mode); + let motors_to_set_sdo = Arc::clone(&self.motors_to_set_sdo); thread::spawn(move || { let mut motors = motors.lock().unwrap(); @@ -1070,6 +1103,17 @@ g } } + { + // Send SDO commands to motors. + let mut motors_to_set_sdo = motors_to_set_sdo.lock().unwrap(); + if !motors_to_set_sdo.is_empty() { + for (motor_id, params) in motors_to_set_sdo.iter_mut() { + motors.set_torque_limit(*motor_id, params.torque_limit).unwrap(); + } + motors_to_set_sdo.clear(); + } + } + { // Send PD commands to motors. let target_params = target_params.read().unwrap(); @@ -1286,6 +1330,11 @@ g }) } + pub fn set_torque_limit(&self, motor_id: u8, torque_limit: f32) -> Result<(), std::io::Error> { + let mut motors = self.motors.lock().unwrap(); + motors.set_torque_limit(motor_id, torque_limit) + } + pub fn add_motor_to_zero(&self, motor_id: u8) -> Result<(), std::io::Error> { // We need to set the motor parameters to zero to avoid the motor // rapidly changing to the new target after it is zeroed.