compare

kscalelabs · Nov 11, 2024 · 9475756 · 9475756
1 parent c85a644
commit 9475756
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 20 deletions.
diff --git a/actuator/robstride/src/config.rs b/actuator/robstride/src/config.rs
@@ -34,7 +34,7 @@ lazy_static! {
                 kp_max: 500.0,
                 kd_min: 0.0,
                 kd_max: 5.0,
-                t_min: -14.0,
+                t_min: 0.0,
                 t_max: 14.0,
                 zero_on_init: false,
                 can_timeout_command: 0x200b, // Unchecked
@@ -52,7 +52,7 @@ lazy_static! {
                 kp_max: 500.0,
                 kd_min: 0.0,
                 kd_max: 5.0,
-                t_min: -12.0,
+                t_min: 0.0,
                 t_max: 12.0,
                 zero_on_init: true, // Single encoder motor.
                 can_timeout_command: 0x200c,
@@ -70,7 +70,7 @@ lazy_static! {
                 kp_max: 500.0,
                 kd_min: 0.0,
                 kd_max: 5.0,
-                t_min: -12.0,
+                t_min: 0.0,
                 t_max: 12.0,
                 zero_on_init: false,
                 can_timeout_command: 0x200b, // Unchecked
@@ -88,7 +88,7 @@ lazy_static! {
                 kp_max: 5000.0,
                 kd_min: 0.0,
                 kd_max: 100.0,
-                t_min: -60.0,
+                t_min: 0.0,
                 t_max: 60.0,
                 zero_on_init: false,
                 can_timeout_command: 0x200b,
@@ -106,7 +106,7 @@ lazy_static! {
                 kp_max: 5000.0,
                 kd_min: 0.0,
                 kd_max: 100.0,
-                t_min: -120.0,
+                t_min: 0.0,
                 t_max: 120.0,
                 zero_on_init: false,
                 can_timeout_command: 0x200b,

diff --git a/actuator/robstride/src/motor.rs b/actuator/robstride/src/motor.rs
@@ -33,11 +33,19 @@ impl Default for MotorControlParams {
     }
 }
 
-#[derive(Debug, Default, Clone, Serialize, Deserialize)]
+#[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,
@@ -364,11 +372,13 @@ impl Motors {
             len: 8,
             data: vec![0; 8],
         };
-        let index: u16 = 0x700b;
+
+        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());
+        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(())
     }

diff --git a/actuator/robstride/src/supervisor.rs b/actuator/robstride/src/supervisor.rs
@@ -158,17 +158,17 @@ 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();
-                    }
-                }
+                // {
+                //     // 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 = {