speed and current limit

kscalelabs · Nov 12, 2024 · f8ada57 · f8ada57
1 parent ff417cd
commit f8ada57
Show file tree

Hide file tree

Showing 3 changed files with 68 additions and 13 deletions.
diff --git 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<f32> {
+        self.inner.set_torque_limit(motor_id, torque_limit).map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        Ok(torque_limit)
+    }
+
+    fn set_speed_limit(&mut self, motor_id: u8, speed_limit: f32) -> PyResult<f32> {
+        self.inner.set_speed_limit(motor_id, speed_limit).map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+        Ok(speed_limit)
+    }
+
+    fn set_current_limit(&mut self, motor_id: u8, current_limit: f32) -> PyResult<f32> {
+        self.inner.set_current_limit(motor_id, current_limit).map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(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::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
     }
 
+    fn set_speed_limit(&self, motor_id: u8, speed_limit: f32) -> PyResult<f32> {
+        self.inner
+            .set_speed_limit(motor_id, speed_limit)
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
+    }
+
+    fn set_current_limit(&self, motor_id: u8, current_limit: f32) -> PyResult<f32> {
+        self.inner
+            .set_current_limit(motor_id, current_limit)
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(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
@@ -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<f32>,
+    pub speed_limit: Option<f32>,
+    pub current_limit: Option<f32>,
 }
 
 #[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
@@ -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<f32, std::io::Error> {
         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<f32, std::io::Error> {
+        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<f32, std::io::Error> {
+        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<f32, std::io::Error> {
         let mut target_params = self.target_params.write().unwrap();
         if let Some(params) = target_params.get_mut(&motor_id) {