diff --git a/actuator/bindings/src/lib.rs b/actuator/bindings/src/lib.rs index 0c328f3..92a2e04 100644 --- a/actuator/bindings/src/lib.rs +++ b/actuator/bindings/src/lib.rs @@ -52,6 +52,21 @@ impl PyRobstrideMotors { .collect() } + fn set_torque_limit(&mut 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()))?; + Ok(torque_limit) + } + + fn set_speed_limit(&mut self, motor_id: u8, speed_limit: f32) -> PyResult { + self.inner.set_speed_limit(motor_id, speed_limit).map_err(|e| PyErr::new::(e.to_string()))?; + Ok(speed_limit) + } + + fn set_current_limit(&mut self, motor_id: u8, current_limit: f32) -> PyResult { + self.inner.set_current_limit(motor_id, current_limit).map_err(|e| PyErr::new::(e.to_string()))?; + Ok(current_limit) + } + fn send_resets(&mut self) -> PyResult<()> { self.inner .send_resets() @@ -351,6 +366,18 @@ impl PyRobstrideMotorsSupervisor { .map_err(|e| PyErr::new::(e.to_string())) } + fn set_speed_limit(&self, motor_id: u8, speed_limit: f32) -> PyResult { + self.inner + .set_speed_limit(motor_id, speed_limit) + .map_err(|e| PyErr::new::(e.to_string())) + } + + fn set_current_limit(&self, motor_id: u8, current_limit: f32) -> PyResult { + self.inner + .set_current_limit(motor_id, current_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 9b75c20..1654572 100644 --- a/actuator/robstride/src/motor.rs +++ b/actuator/robstride/src/motor.rs @@ -35,15 +35,9 @@ 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, - } - } + pub torque_limit: Option, + pub speed_limit: Option, + pub current_limit: Option, } #[derive(Debug, Default, Clone, Serialize, Deserialize)] @@ -356,7 +350,7 @@ impl Motors { Ok(()) } - pub fn write_sdo_param(&mut self, motor_id: u8, index: u16, value: f32) -> Result<(), std::io::Error> { + fn write_sdo_param(&mut self, motor_id: u8, index: u16, value: f32) -> Result<(), std::io::Error> { let mut pack = CanPack { ex_id: ExId { id: motor_id, @@ -376,6 +370,21 @@ impl Motors { Ok(()) } + // Set speed limit in rad/s + pub fn set_speed_limit(&mut self, motor_id: u8, speed_limit: f32) -> Result<(), std::io::Error> { + let index: u16 = 0x7017; + self.write_sdo_param(motor_id, index, speed_limit)?; + Ok(()) + } + + // Set current limit in A + pub fn set_current_limit(&mut self, motor_id: u8, current_limit: f32) -> Result<(), std::io::Error> { + let index: u16 = 0x7018; + self.write_sdo_param(motor_id, index, current_limit)?; + Ok(()) + } + + // Set torque limit in Nm pub fn set_torque_limit(&mut self, motor_id: u8, torque_limit: f32) -> Result<(), std::io::Error> { let index: u16 = 0x700B; // optionally clamp to min/max diff --git a/actuator/robstride/src/supervisor.rs b/actuator/robstride/src/supervisor.rs index 403b22a..b09d7ff 100644 --- a/actuator/robstride/src/supervisor.rs +++ b/actuator/robstride/src/supervisor.rs @@ -163,8 +163,15 @@ impl MotorsSupervisor { 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. + if let Some(torque_limit) = params.torque_limit { + motors.set_torque_limit(*motor_id, torque_limit).unwrap(); + } + if let Some(speed_limit) = params.speed_limit { + motors.set_speed_limit(*motor_id, speed_limit).unwrap(); + } + if let Some(current_limit) = params.current_limit { + motors.set_current_limit(*motor_id, current_limit).unwrap(); + } } motors_to_set_sdo.clear(); } @@ -391,10 +398,22 @@ 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 }); + motors_to_set_sdo.insert(motor_id, MotorSdoParams { torque_limit: Some(torque_limit), speed_limit: None, current_limit: None }); Ok(torque_limit) } + pub fn set_speed_limit(&self, motor_id: u8, speed_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: None, speed_limit: Some(speed_limit), current_limit: None }); + Ok(speed_limit) + } + + pub fn set_current_limit(&self, motor_id: u8, current_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: None, speed_limit: None, current_limit: Some(current_limit) }); + Ok(current_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) {