zero on init (#33)

kscalelabs · Oct 18, 2024 · 0a19ccf · 0a19ccf
1 parent 539bcee
commit 0a19ccf
Show file tree

Hide file tree

Showing 4 changed files with 30 additions and 8 deletions.
diff --git a/actuator/rust/bindings/src/lib.rs b/actuator/rust/bindings/src/lib.rs
@@ -193,12 +193,13 @@ struct PyRobstrideMotorsSupervisor {
 #[pymethods]
 impl PyRobstrideMotorsSupervisor {
     #[new]
-    #[pyo3(signature = (port_name, motor_infos, verbose = false, target_update_rate = 50.0))]
+    #[pyo3(signature = (port_name, motor_infos, verbose = false, target_update_rate = 50.0, zero_on_init = false))]
     fn new(
         port_name: String,
         motor_infos: HashMap<u8, String>,
         verbose: bool,
         target_update_rate: f64,
+        zero_on_init: bool,
     ) -> PyResult<Self> {
         let motor_infos = motor_infos
             .into_iter()
@@ -209,8 +210,14 @@ impl PyRobstrideMotorsSupervisor {
             .collect::<PyResult<HashMap<u8, RobstrideMotorType>>>()?;
 
         let controller =
-            RobstrideMotorsSupervisor::new(&port_name, &motor_infos, verbose, target_update_rate)
-                .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
+            RobstrideMotorsSupervisor::new(
+                &port_name,
+                &motor_infos,
+                verbose,
+                target_update_rate,
+                zero_on_init,
+            )
+            .map_err(|e| PyErr::new::<pyo3::exceptions::PyRuntimeError, _>(e.to_string()))?;
 
         Ok(PyRobstrideMotorsSupervisor { inner: controller })
     }

diff --git a/actuator/rust/robstride/src/bin/multisupervisor.rs b/actuator/rust/robstride/src/bin/multisupervisor.rs
@@ -14,6 +14,8 @@ struct Args {
     min_update_rate: f64,
     #[arg(long, help = "Maximum update rate (Hz)", default_value_t = 1000.0)]
     max_update_rate: f64,
+    #[arg(long, help = "Zero on init", default_value_t = false)]
+    zero_on_init: bool,
 }
 
 fn sinusoid(
@@ -123,8 +125,13 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
         })
         .collect();
 
-    let controller =
-        MotorsSupervisor::new(&port_name, &motor_types, args.verbose, args.max_update_rate)?;
+    let controller = MotorsSupervisor::new(
+        &port_name,
+        &motor_types,
+        args.verbose,
+        args.max_update_rate,
+        args.zero_on_init,
+    )?;
 
     println!("Motor Controller Test CLI");
     println!("Available commands:");

diff --git a/actuator/rust/robstride/src/bin/supervisor.rs b/actuator/rust/robstride/src/bin/supervisor.rs
@@ -14,6 +14,8 @@ struct Args {
     min_update_rate: f64,
     #[arg(long, help = "Maximum update rate (Hz)", default_value_t = 1000.0)]
     max_update_rate: f64,
+    #[arg(long, help = "Zero on init", default_value_t = false)]
+    zero_on_init: bool,
 }
 
 fn sinusoid(
@@ -85,8 +87,13 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
     };
     let motor_type = motor_type_from_str(motor_type_input.as_str())?;
     let motor_infos: HashMap<u8, MotorType> = HashMap::from([(test_id, motor_type)]);
-    let controller =
-        MotorsSupervisor::new(&port_name, &motor_infos, args.verbose, args.max_update_rate)?;
+    let controller = MotorsSupervisor::new(
+        &port_name,
+        &motor_infos,
+        args.verbose,
+        args.max_update_rate,
+        args.zero_on_init,
+    )?;
 
     println!("Motor Controller Test CLI");
     println!("Available commands:");

diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs
@@ -899,6 +899,7 @@ impl MotorsSupervisor {
         motor_infos: &HashMap<u8, MotorType>,
         verbose: bool,
         max_update_rate: f64,
+        zero_on_init: bool,
     ) -> Result<Self, Box<dyn std::error::Error>> {
         // Initialize Motors
         let motors = Motors::new(port_name, motor_infos, verbose)?;
@@ -925,7 +926,7 @@ impl MotorsSupervisor {
         let zero_on_init_motors = motors
             .motor_configs
             .iter()
-            .filter(|(_, &config)| config.zero_on_init)
+            .filter(|(_, &config)| config.zero_on_init || zero_on_init)
             .map(|(&id, _)| id)
             .collect::<HashSet<u8>>();