diff --git a/actuator/bindings/src/lib.rs b/actuator/bindings/src/lib.rs index 07014e9..0c328f3 100644 --- a/actuator/bindings/src/lib.rs +++ b/actuator/bindings/src/lib.rs @@ -345,6 +345,12 @@ impl PyRobstrideMotorsSupervisor { .map_err(|e| PyErr::new::(e.to_string())) } + fn set_torque_limit(&self, motor_id: u8, torque_limit: f32) -> PyResult { + self.inner + .set_torque_limit(motor_id, torque_limit) + .map_err(|e| PyErr::new::(e.to_string())) + } + fn add_motor_to_zero(&self, motor_id: u8) -> PyResult<()> { self.inner .add_motor_to_zero(motor_id) diff --git a/actuator/robstride/src/motor.rs b/actuator/robstride/src/motor.rs index 34f5c93..df8b77c 100644 --- a/actuator/robstride/src/motor.rs +++ b/actuator/robstride/src/motor.rs @@ -33,6 +33,19 @@ impl Default for MotorControlParams { } } +#[derive(Debug, Clone, Serialize, Deserialize)] +pub struct MotorSdoParams { + pub torque_limit: f32, +} + +impl Default for MotorSdoParams { + fn default() -> Self { + MotorSdoParams { + torque_limit: 0.0, + } + } +} + #[derive(Debug, Default, Clone, Serialize, Deserialize)] pub struct MotorFeedback { pub can_id: u8, @@ -343,6 +356,33 @@ impl Motors { Ok(()) } + pub 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_safe = torque_limit.clamp(config.t_min, config.t_max); + pack.data[4..8].copy_from_slice(&torque_limit_safe.to_le_bytes()); + + self.send_command(&pack, true)?; + Ok(()) + } + pub fn send_reset(&mut self, id: u8) -> Result { let pack = CanPack { ex_id: ExId { diff --git a/actuator/robstride/src/supervisor.rs b/actuator/robstride/src/supervisor.rs index b567b65..403b22a 100644 --- a/actuator/robstride/src/supervisor.rs +++ b/actuator/robstride/src/supervisor.rs @@ -3,7 +3,7 @@ use std::sync::{Arc, Mutex, RwLock}; use std::thread; use std::time::Duration; -use crate::motor::{MotorControlParams, MotorFeedback, Motors}; +use crate::motor::{MotorControlParams, MotorFeedback, MotorSdoParams, Motors}; use crate::types::{MotorType, RunMode}; use log::{error, info}; @@ -13,6 +13,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>, @@ -69,6 +70,7 @@ impl MotorsSupervisor { 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)), @@ -91,6 +93,7 @@ impl MotorsSupervisor { let motors_to_zero = Arc::clone(&self.motors_to_zero); let paused = Arc::clone(&self.paused); let restart = Arc::clone(&self.restart); + let motors_to_set_sdo = Arc::clone(&self.motors_to_set_sdo); let total_commands = Arc::clone(&self.total_commands); let failed_commands = Arc::clone(&self.failed_commands); let max_update_rate = Arc::clone(&self.max_update_rate); @@ -155,6 +158,18 @@ impl MotorsSupervisor { } } + { + // Send updated sdo parameters to motors that need them. + 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(); + // Any other sdo parameters can be updated here. + } + motors_to_set_sdo.clear(); + } + } + { let params_copy = { let target_params = target_params.read().unwrap(); @@ -374,6 +389,12 @@ impl MotorsSupervisor { }) } + pub fn set_torque_limit(&self, motor_id: u8, torque_limit: f32) -> Result { + let mut motors_to_set_sdo = self.motors_to_set_sdo.lock().unwrap(); + motors_to_set_sdo.insert(motor_id, MotorSdoParams { torque_limit }); + Ok(torque_limit) + } + pub fn set_torque(&self, motor_id: u8, torque: f32) -> Result { let mut target_params = self.target_params.write().unwrap(); if let Some(params) = target_params.get_mut(&motor_id) { diff --git a/examples/supervisor.py b/examples/supervisor.py index 3607012..a696d38 100644 --- a/examples/supervisor.py +++ b/examples/supervisor.py @@ -9,9 +9,9 @@ def main() -> None: parser = argparse.ArgumentParser() - parser.add_argument("--port-name", type=str, default="/dev/ttyUSB0") + parser.add_argument("--port-name", type=str, default="/dev/ttyCH341USB0") parser.add_argument("--motor-id", type=int, default=1) - parser.add_argument("--motor-type", type=str, default="01") + parser.add_argument("--motor-type", type=str, default="04") parser.add_argument("--sleep", type=float, default=0.0) parser.add_argument("--period", type=float, default=10.0) parser.add_argument("--amplitude", type=float, default=1.0)