motor script + improved supervisor script

actuator/rust/robstride/src/bin/motors.rs

@@ -18,8 +18,13 @@ fn sinusoid(
     amplitude: f32,
     duration: Duration,
 ) -> Result<(), Box<dyn std::error::Error>> {
-    let start = Instant::now();
+    let _ = motors.send_resets();
+    let _ = motors.send_starts();
+    let _ = motors.zero_motors(&[id]);
 
+    let start = Instant::now();
+    let mut command_count = 0;
+    let mut last_second = start;
     while start.elapsed() < duration {
         let t = start.elapsed().as_secs_f32();
         let pos = amplitude * (2.0 * PI * t).sin();
@@ -36,8 +41,21 @@ fn sinusoid(
             )]),
             true,
         )?;
+
+        command_count += 1;
+
+        // Check if a second has passed
+        if last_second.elapsed() > Duration::from_secs(1) {
+            println!(
+                "Commands per second: {}",
+                command_count as f32 / start.elapsed().as_secs_f32()
+            );
+            last_second = Instant::now();
+        }
     }
 
+    let _ = motors.send_resets();
+
     Ok(())
 }
 
@@ -77,16 +95,8 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
 
     println!("Motor Controller Test CLI");
     println!("Available commands:");
-    println!("  p <position>");
-    println!("  v <velocity>");
-    println!("  t <torque>");
-    println!("  kp <kp>");
-    println!("  kd <kd>");
-    println!("  sinusoid / s");
+    println!("  sinusoid / s (<duration>)");
     println!("  zero / z");
-    println!("  get_feedback / g");
-    println!("  pause / w");
-    println!("  reset / r");
     println!("  quit / q");
 
     loop {
@@ -103,7 +113,12 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
 
         match parts[0] {
             "sinusoid" | "s" => {
-                let _ = sinusoid(&mut motors, test_id, 1.0, Duration::from_secs(1));
+                let duration = Duration::from_secs(if parts.len() == 2 {
+                    parts[1].parse::<u64>()?
+                } else {
+                    1
+                });
+                let _ = sinusoid(&mut motors, test_id, 1.0, duration);
                 println!("Ran motor {} sinusoid test", test_id);
             }
             "zero" | "z" => {

actuator/rust/robstride/src/bin/supervisor.rs

@@ -24,18 +24,30 @@ fn sinusoid(
     amplitude: f32,
     duration: Duration,
 ) -> Result<(), Box<dyn std::error::Error>> {
-    let start = Instant::now();
-
     controller.set_kd(id, 1.0)?;
     controller.set_kp(id, 10.0)?;
     controller.set_velocity(id, 0.0)?;
     controller.set_torque(id, 0.0)?;
 
+    let start = Instant::now();
+    let mut last_second = start;
+    controller.reset_command_counters();
+
     while start.elapsed() < duration {
         let t = start.elapsed().as_secs_f32();
         let pos = amplitude * (2.0 * PI * t).sin();
         controller.set_position(id, pos)?;
         std::thread::sleep(Duration::from_millis(10));
+
+        // Check if a second has passed
+        let total_commands = controller.get_total_commands();
+        if last_second.elapsed() > Duration::from_secs(1) {
+            println!(
+                "Commands per second: {}",
+                total_commands as f32 / start.elapsed().as_secs_f32()
+            );
+            last_second = Instant::now();
+        }
     }
 
     controller.set_position(id, 0.0)?;
@@ -90,7 +102,7 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
     println!("  t <torque>");
     println!("  kp <kp>");
     println!("  kd <kd>");
-    println!("  sinusoid / s");
+    println!("  sinusoid / s (<duration>)");
     println!("  zero / z");
     println!("  get_feedback / g");
     println!("  pause / w");
@@ -156,7 +168,12 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
                 println!("Set KD for motor {} to {}", test_id, kd);
             }
             "sinusoid" | "s" => {
-                let _ = sinusoid(&controller, test_id, 1.0, Duration::from_secs(1));
+                let duration = Duration::from_secs(if parts.len() == 2 {
+                    parts[1].parse::<u64>()?
+                } else {
+                    1
+                });
+                let _ = sinusoid(&controller, test_id, 1.0, duration);
                 println!("Ran motor {} sinusoid test", test_id);
             }
             "zero" | "z" => {

actuator/rust/robstride/src/lib.rs

@@ -637,13 +637,7 @@ impl Motors {
     }
 
     pub fn zero_motors(&mut self, motor_ids: &[u8]) -> Result<(), std::io::Error> {
-        self.send_motor_controls(
-            &motor_ids
-                .iter()
-                .map(|&id| (id, MotorControlParams::default()))
-                .collect::<HashMap<u8, MotorControlParams>>(),
-            true,
-        )?;
+        self.send_zero_torque(motor_ids)?;
         self.send_set_zeros(Some(motor_ids))?;
         Ok(())
     }
@@ -930,6 +924,16 @@ impl Motors {
             .collect::<HashMap<u8, MotorFeedback>>())
     }
 
+    pub fn send_zero_torque(&mut self, motor_ids: &[u8]) -> Result<(), std::io::Error> {
+        let params = HashMap::from_iter(
+            motor_ids
+                .iter()
+                .map(|&id| (id, MotorControlParams::default())),
+        );
+        self.send_motor_controls(&params, true)?;
+        Ok(())
+    }
+
     fn unpack_feedback(&mut self, pack: &CanPack) -> Result<MotorFeedback, std::io::Error> {
         let raw_feedback = unpack_raw_feedback(pack);