diff --git a/actuator/robstride/src/config.rs b/actuator/robstride/src/config.rs index 2bd7188..baea106 100644 --- 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 index 55bf28f..df8b77c 100644 --- 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 index 403b22a..1cec2b0 100644 --- 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 = {