clean

kscalelabs · Oct 9, 2024 · d051825 · d051825
1 parent 469e4bf
commit d051825
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 52 deletions.
diff --git a/actuator/rust/robstride/src/bin/profile.rs b/actuator/rust/robstride/src/bin/profile.rs
@@ -97,15 +97,6 @@ fn main() -> Result<(), Box<dyn Error>> {
         // Read feedback from the motors
         motors.read_all_pending_responses()?;
 
-        // // Print the latest feedback for each motor
-        // if let Some(feedback) = motors.get_latest_feedback(1) {
-        //     println!("Motor 1 Feedback: {:?}", feedback);
-        // }
-
-        // if let Some(feedback) = motors.get_latest_feedback(2) {
-        //     println!("Motor 2 Feedback: {:?}", feedback);
-        // }
-
         // Calculate and print the command rate
         let commands_per_second = command_count as f32 / elapsed_time;
         println!("Commands per second: {:.2}", commands_per_second);

diff --git a/actuator/rust/robstride/src/bin/stand.rs b/actuator/rust/robstride/src/bin/stand.rs
@@ -1,4 +1,6 @@
-use robstride::{Motor, Motors, RunMode, ROBSTRIDE_CONFIGS};
+// Script to freeze the legs on Stompy Pro (5 dof per leg). Each motor has a sequentially increasing ID from 1 to 5.
+
+use robstride::{Motor, Motors, ROBSTRIDE_CONFIGS};
 use std::collections::HashMap;
 use std::error::Error;
 use std::sync::atomic::{AtomicBool, Ordering};
@@ -32,29 +34,25 @@ fn main() -> Result<(), Box<dyn Error>> {
 
     // Create Motors instances with the port names
     let mut motors_1 = Motors::new("/dev/ttyCH341USB0", motors_map_1)?;
-    std::thread::sleep(Duration::from_millis(100));
     let mut motors_2 = Motors::new("/dev/ttyCH341USB1", motors_map_2)?;
-    std::thread::sleep(Duration::from_millis(100));
+
     // Function to initialize motors
     let initialize_motors = |motors: &mut Motors| -> Result<(), Box<dyn Error>> {
         for id in 1..=5 {
             for _ in 0..3 {
                 motors.send_reset(id)?;
                 std::thread::sleep(Duration::from_millis(50));
             }
-            std::thread::sleep(Duration::from_millis(100));
 
             for _ in 0..3 {
                 motors.send_start(id)?;
                 std::thread::sleep(Duration::from_millis(50));
             }
-            std::thread::sleep(Duration::from_millis(100));
 
             for _ in 0..3 {
                 motors.send_set_zero(id)?;
                 std::thread::sleep(Duration::from_millis(50));
             }
-            std::thread::sleep(Duration::from_millis(100));
 
             for _ in 0..3 {
                 motors.send_set_location(id, 0.0)?;
@@ -67,8 +65,8 @@ fn main() -> Result<(), Box<dyn Error>> {
 
     let set_location = |motors: &mut Motors, location: f32, kp: f32, kd: f32| -> Result<(), Box<dyn Error>> {
         for id in 1..=5 {
-            retry_until_ok(|| motors.send_position_control(id, location, kp, kd).map_err(|e| Box::new(e) as Box<dyn Error>))?;
-            std::thread::sleep(Duration::from_millis(100));
+            motors.send_position_control(id, location, kp, kd)?;
+            std::thread::sleep(Duration::from_millis(50));
         }
         motors.read_all_pending_responses()?;
         Ok(())
@@ -84,12 +82,10 @@ fn main() -> Result<(), Box<dyn Error>> {
 
     set_location(&mut motors_1, 0.0, 180.0, 2.0)?;
     set_location(&mut motors_2, 0.0, 50.0, 0.4)?;
+
     // Wait until interrupted
     while running.load(Ordering::SeqCst) {
-    //     std::thread::sleep(Duration::from_millis(100));
-    //     set_location(&mut motors_1, 0.0, 60.0)?;
         std::thread::sleep(Duration::from_millis(100));
-    //     set_location(&mut motors_2, 0.0, 30.0)?;
     }
 
     // Reset motors on exit
@@ -103,25 +99,3 @@ fn main() -> Result<(), Box<dyn Error>> {
 
     Ok(())
 }
-
-fn retry_until_ok<F>(mut operation: F) -> Result<(), Box<dyn Error>>
-where
-    F: FnMut() -> Result<(), Box<dyn Error>>,
-{
-    let max_retries = 5;
-    let mut attempts = 0;
-
-    loop {
-        match operation() {
-            Ok(_) => return Ok(()),
-            Err(e) => {
-                println!("Error: {}", e);
-                attempts += 1;
-                if attempts >= max_retries {
-                    return Err(e);
-                }
-                std::thread::sleep(Duration::from_millis(100)); // Wait before retrying
-            }
-        }
-    }
-}
diff --git a/actuator/rust/robstride/src/main.rs b/actuator/rust/robstride/src/main.rs
@@ -7,6 +7,7 @@ use std::sync::Arc;
 use std::time::{Duration, Instant};
 use ctrlc;
 
+const TEST_ID: u8 = 1;
 fn main() -> Result<(), Box<dyn Error>> {
     // Create an atomic flag to handle SIGINT
     let running = Arc::new(AtomicBool::new(true));
@@ -27,15 +28,15 @@ fn main() -> Result<(), Box<dyn Error>> {
     // Create a Motors instance with the port name
     let mut motors = Motors::new("/dev/ttyCH341USB0", motors_map)?; // Adjust the device path as needed
 
-    motors.send_reset(1)?;
+    motors.send_reset(TEST_ID)?;
     std::thread::sleep(Duration::from_millis(50));
-    motors.send_set_mode(1, RunMode::MitMode)?;
+    motors.send_set_mode(TEST_ID, RunMode::MitMode)?;
     std::thread::sleep(Duration::from_millis(50));
-    motors.send_start(1)?;
+    motors.send_start(TEST_ID)?;
     std::thread::sleep(Duration::from_millis(50));
-    motors.send_set_speed_limit(1, 10.0)?;
+    motors.send_set_speed_limit(TEST_ID, 10.0)?;
     std::thread::sleep(Duration::from_millis(50));
-    motors.send_set_zero(1)?;
+    motors.send_set_zero(TEST_ID)?;
 
     motors.read_all_pending_responses()?;
 
@@ -66,7 +67,7 @@ fn main() -> Result<(), Box<dyn Error>> {
         let torque_1 = kp_04 * (desired_position_1 - current_position_1) - kd_04 * current_velocity_1;
 
         // Send torque commands to the motors
-        motors.send_torque_control(1, torque_1 as f32)?;
+        motors.send_torque_control(TEST_ID, torque_1 as f32)?;
         std::thread::sleep(Duration::from_millis(4)); // Sleep to prevent overwhelming the bus
 
         // Increment the command counter
@@ -75,10 +76,10 @@ fn main() -> Result<(), Box<dyn Error>> {
         // Read feedback from the motors
         motors.read_all_pending_responses()?;
 
-        // // Print the latest feedback for each motor
-        // if let Some(feedback) = motors.get_latest_feedback(1) {
-        //     println!("Motor 1 Feedback: {:?}", feedback);
-        // }
+        // Print the latest feedback for each motor
+        if let Some(feedback) = motors.get_latest_feedback(TEST_ID) {
+            println!("Motor 1 Feedback: {:?}", feedback);
+        }
 
         // Calculate and print the command rate
         let commands_per_second = command_count as f32 / elapsed_time;