From c85a644b3616822fcb24b249e2dc506902ba7d37 Mon Sep 17 00:00:00 2001
From: WT-MM <wesley.maa@gmail.com>
Date: Mon, 11 Nov 2024 11:50:12 -0800
Subject: [PATCH] torque limit draft

---
 actuator/bindings/src/lib.rs         |  6 ++++++
 actuator/robstride/src/motor.rs      | 30 ++++++++++++++++++++++++++++
 actuator/robstride/src/supervisor.rs | 23 ++++++++++++++++++++-
 3 files changed, 58 insertions(+), 1 deletion(-)

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::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))
     }
 
+    fn set_torque_limit(&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()))
+    }
+
     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..55bf28f 100644
--- a/actuator/robstride/src/motor.rs
+++ b/actuator/robstride/src/motor.rs
@@ -33,6 +33,11 @@ impl Default for MotorControlParams {
     }
 }
 
+#[derive(Debug, Default, Clone, Serialize, Deserialize)]
+pub struct MotorSdoParams {
+    pub torque_limit: f32,
+}
+
 #[derive(Debug, Default, Clone, Serialize, Deserialize)]
 pub struct MotorFeedback {
     pub can_id: u8,
@@ -343,6 +348,31 @@ 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_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(())
+    }
+
     pub fn send_reset(&mut self, id: u8) -> Result<CanPack, std::io::Error> {
         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<RwLock<bool>>,
     latest_feedback: Arc<RwLock<HashMap<u8, MotorFeedback>>>,
     motors_to_zero: Arc<Mutex<HashSet<u8>>>,
+    motors_to_set_sdo: Arc<Mutex<HashMap<u8, MotorSdoParams>>>,
     paused: Arc<RwLock<bool>>,
     restart: Arc<Mutex<bool>>,
     total_commands: Arc<RwLock<u64>>,
@@ -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<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 });
+        Ok(torque_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) {