diff --git a/actuator/rust/bindings/bindings.pyi b/actuator/rust/bindings/bindings.pyi index 7bbb413..b580b1e 100644 --- a/actuator/rust/bindings/bindings.pyi +++ b/actuator/rust/bindings/bindings.pyi @@ -53,7 +53,7 @@ class PyRobstrideMotors: class PyRobstrideMotorsSupervisor: - def __new__(cls,port_name,motor_infos,verbose = ...): ... + def __new__(cls,port_name,motor_infos,verbose = ...,min_update_rate = ...,target_update_rate = ...): ... def set_position(self, motor_id:int, position:float) -> None: ... @@ -69,9 +69,6 @@ class PyRobstrideMotorsSupervisor: def set_torque(self, motor_id:int, torque:float) -> None: ... - def set_sleep_duration(self, sleep_duration:float) -> None: - ... - def add_motor_to_zero(self, motor_id:int) -> None: ... @@ -99,4 +96,13 @@ class PyRobstrideMotorsSupervisor: def reset_command_counters(self) -> None: ... + def set_min_update_rate(self, rate:float) -> None: + ... + + def set_target_update_rate(self, rate:float) -> None: + ... + + def get_actual_update_rate(self) -> float: + ... + diff --git a/actuator/rust/bindings/src/lib.rs b/actuator/rust/bindings/src/lib.rs index b4c2872..b8d9728 100644 --- a/actuator/rust/bindings/src/lib.rs +++ b/actuator/rust/bindings/src/lib.rs @@ -8,7 +8,6 @@ use robstride::{ MotorsSupervisor as RobstrideMotorsSupervisor, }; use std::collections::HashMap; -use std::time::Duration; #[gen_stub_pyclass] #[pyclass] @@ -227,8 +226,14 @@ struct PyRobstrideMotorsSupervisor { #[pymethods] impl PyRobstrideMotorsSupervisor { #[new] - #[pyo3(signature = (port_name, motor_infos, verbose = false))] - fn new(port_name: String, motor_infos: HashMap, verbose: bool) -> PyResult { + #[pyo3(signature = (port_name, motor_infos, verbose = false, min_update_rate = 10.0, target_update_rate = 50.0))] + fn new( + port_name: String, + motor_infos: HashMap, + verbose: bool, + min_update_rate: f64, + target_update_rate: f64, + ) -> PyResult { let motor_infos = motor_infos .into_iter() .map(|(id, type_str)| { @@ -237,8 +242,14 @@ impl PyRobstrideMotorsSupervisor { }) .collect::>>()?; - let controller = RobstrideMotorsSupervisor::new(&port_name, &motor_infos, verbose) - .map_err(|e| PyErr::new::(e.to_string()))?; + let controller = RobstrideMotorsSupervisor::new( + &port_name, + &motor_infos, + verbose, + min_update_rate, + target_update_rate, + ) + .map_err(|e| PyErr::new::(e.to_string()))?; Ok(PyRobstrideMotorsSupervisor { inner: controller }) } @@ -268,12 +279,6 @@ impl PyRobstrideMotorsSupervisor { Ok(()) } - fn set_sleep_duration(&self, sleep_duration: f32) -> PyResult<()> { - self.inner - .set_sleep_duration(Duration::from_millis(sleep_duration as u64)); - Ok(()) - } - fn add_motor_to_zero(&self, motor_id: u8) -> PyResult<()> { self.inner.add_motor_to_zero(motor_id); Ok(()) @@ -331,6 +336,20 @@ impl PyRobstrideMotorsSupervisor { self.inner.reset_command_counters(); Ok(()) } + + fn set_min_update_rate(&self, rate: f64) -> PyResult<()> { + self.inner.set_min_update_rate(rate); + Ok(()) + } + + fn set_target_update_rate(&self, rate: f64) -> PyResult<()> { + self.inner.set_target_update_rate(rate); + Ok(()) + } + + fn get_actual_update_rate(&self) -> PyResult { + Ok(self.inner.get_actual_update_rate()) + } } #[pymodule] diff --git a/actuator/rust/robstride/src/bin/supervisor.rs b/actuator/rust/robstride/src/bin/supervisor.rs index 9ffa33a..33c103d 100644 --- a/actuator/rust/robstride/src/bin/supervisor.rs +++ b/actuator/rust/robstride/src/bin/supervisor.rs @@ -8,6 +8,10 @@ use std::io::{self, Write}; struct Args { #[arg(short, long, help = "Enable verbose output")] verbose: bool, + #[arg(short, long, help = "Minimum update rate (Hz)", default_value_t = 10.0)] + min_update_rate: f64, + #[arg(short, long, help = "Target update rate (Hz)", default_value_t = 50.0)] + target_update_rate: f64, } fn main() -> Result<(), Box> { @@ -42,7 +46,13 @@ fn main() -> Result<(), Box> { }; let motor_type = motor_type_from_str(motor_type_input.as_str())?; let motor_infos: HashMap = HashMap::from([(test_id, motor_type)]); - let controller = MotorsSupervisor::new(&port_name, &motor_infos, args.verbose)?; + let controller = MotorsSupervisor::new( + &port_name, + &motor_infos, + args.verbose, + args.min_update_rate, + args.target_update_rate, + )?; println!("Motor Controller Test CLI"); println!("Available commands:"); diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index ed89bda..456bcda 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -898,11 +898,13 @@ pub struct MotorsSupervisor { running: Arc>, latest_feedback: Arc>>, motors_to_zero: Arc>>, - sleep_duration: Arc>, paused: Arc>, restart: Arc>, total_commands: Arc>, failed_commands: Arc>, + min_update_rate: Arc>, + target_update_rate: Arc>, + actual_update_rate: Arc>, } impl MotorsSupervisor { @@ -910,6 +912,8 @@ impl MotorsSupervisor { port_name: &str, motor_infos: &HashMap, verbose: bool, + min_update_rate: f64, + target_update_rate: f64, ) -> Result> { // Initialize Motors let motors = Motors::new(port_name, motor_infos, verbose)?; @@ -953,11 +957,13 @@ impl MotorsSupervisor { running, latest_feedback: Arc::new(Mutex::new(HashMap::new())), motors_to_zero, - sleep_duration: Arc::new(Mutex::new(Duration::from_micros(10))), paused, restart, total_commands: Arc::new(Mutex::new(0)), failed_commands: Arc::new(Mutex::new(0)), + min_update_rate: Arc::new(Mutex::new(min_update_rate)), + target_update_rate: Arc::new(Mutex::new(target_update_rate)), + actual_update_rate: Arc::new(Mutex::new(0.0)), }; controller.start_control_thread(); @@ -971,34 +977,56 @@ impl MotorsSupervisor { let running = Arc::clone(&self.running); let latest_feedback = Arc::clone(&self.latest_feedback); let motors_to_zero = Arc::clone(&self.motors_to_zero); - let sleep_duration = Arc::clone(&self.sleep_duration); let paused = Arc::clone(&self.paused); let restart = Arc::clone(&self.restart); let total_commands = Arc::clone(&self.total_commands); let failed_commands = Arc::clone(&self.failed_commands); + let min_update_rate = Arc::clone(&self.min_update_rate); + let target_update_rate = Arc::clone(&self.target_update_rate); + let actual_update_rate = Arc::clone(&self.actual_update_rate); thread::spawn(move || { let mut motors = motors.lock().unwrap(); let _ = motors.send_reset(); let _ = motors.send_start(); - let _ = motors.send_can_timeout(100); // If motor doesn't receive a command for 100ms, it will stop. - while *running.lock().unwrap() { - // If paused, just wait 100ms without sending any commands. - if *paused.lock().unwrap() { - std::thread::sleep(Duration::from_millis(100)); - continue; + // Set CAN timeout based on minimum update rate + let can_timeout = (1000.0 / *min_update_rate.lock().unwrap()) as u32; + let _ = motors.send_can_timeout(can_timeout); + + let mut last_update_time = std::time::Instant::now(); + + loop { + { + // If not running, break the loop. + if !*running.lock().unwrap() { + break; + } + } + + { + // If paused, just wait a short time without sending any commands. + if *paused.lock().unwrap() { + std::thread::sleep(Duration::from_millis(10)); + continue; + } } - if *restart.lock().unwrap() { - *restart.lock().unwrap() = false; - let _ = motors.send_reset(); - let _ = motors.send_start(); + { + // If restart is requested, reset and restart the motors. + let mut restart = restart.lock().unwrap(); + if *restart { + *restart = false; + let _ = motors.send_reset(); + let _ = motors.send_start(); + } } - // Read latest feedback from motors. + let loop_start_time = std::time::Instant::now(); + { + // Read latest feedback from motors. let latest_feedback_from_motors = motors.get_latest_feedback(); let mut latest_feedback = latest_feedback.lock().unwrap(); *latest_feedback = latest_feedback_from_motors.clone(); @@ -1034,8 +1062,18 @@ impl MotorsSupervisor { *total_commands.lock().unwrap() += 1; } - { - std::thread::sleep(sleep_duration.lock().unwrap().clone()); + // Calculate actual update rate + let elapsed = loop_start_time.duration_since(last_update_time); + last_update_time = loop_start_time; + let current_rate = 1.0 / elapsed.as_secs_f64(); + *actual_update_rate.lock().unwrap() = current_rate; + + // Sleep to maintain target update rate + let target_duration = + Duration::from_secs_f64(1.0 / *target_update_rate.lock().unwrap()); + let elapsed = loop_start_time.elapsed(); + if elapsed < target_duration { + std::thread::sleep(target_duration - elapsed); } } @@ -1109,11 +1147,6 @@ impl MotorsSupervisor { } } - pub fn set_sleep_duration(&self, sleep_duration: Duration) { - let mut sleep_duration_to_set = self.sleep_duration.lock().unwrap(); - *sleep_duration_to_set = sleep_duration; - } - pub fn add_motor_to_zero(&self, motor_id: u8) { // We need to set the motor parameters to zero to avoid the motor // rapidly changing to the new target after it is zeroed. @@ -1146,6 +1179,23 @@ impl MotorsSupervisor { } std::thread::sleep(Duration::from_millis(200)); } + + pub fn set_min_update_rate(&self, rate: f64) { + let mut min_rate = self.min_update_rate.lock().unwrap(); + *min_rate = rate; + let can_timeout = (1000.0 / rate) as u32; + let mut motors = self.motors.lock().unwrap(); + let _ = motors.send_can_timeout(can_timeout); + } + + pub fn set_target_update_rate(&self, rate: f64) { + let mut target_rate = self.target_update_rate.lock().unwrap(); + *target_rate = rate; + } + + pub fn get_actual_update_rate(&self) -> f64 { + *self.actual_update_rate.lock().unwrap() + } } impl Drop for MotorsSupervisor {