standing robot

actuator/rust/README.md

@@ -23,3 +23,17 @@ To run the profiling script:
 ```bash
 cargo run --bin profile
 ```
+
+To run the stand script:
+
+(make sure that the right leg is ttyCH341USB0 and the left leg is ttyCH341USB1)
+
+```bash
+cargo run --bin stand
+```
+
+Also set the baud rate to 921600 with
+
+```bash
+sudo stty -F /dev/ttyCH341USB0 921600
+```

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

@@ -32,55 +32,96 @@ fn main() -> Result<(), Box<dyn Error>> {
 
     // Create Motors instances with the port names
     let mut motors_1 = Motors::new("/dev/ttyCH341USB0", motors_map_1)?;
-    // let mut motors_2 = Motors::new("/dev/ttyCH341USB1", motors_map_2)?;
-
+    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 {
-            motors.send_reset(id)?;
-            // std::thread::sleep(Duration::from_millis(50));
-            // motors.send_set_mode(id, RunMode::MitMode)?;
-            std::thread::sleep(Duration::from_millis(50));
-            motors.send_start(id)?;
-            std::thread::sleep(Duration::from_millis(50));
-            motors.send_set_zero(id)?;
-            std::thread::sleep(Duration::from_millis(50));
-            motors.send_set_location(id, 0.0)?; // Force set location to 0
+            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)?;
+                std::thread::sleep(Duration::from_millis(50));
+            }
         }
         motors.read_all_pending_responses()?;
         Ok(())
     };
 
-    let set_location = |motors: &mut Motors, location: f32| -> 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 {
-            motors.send_position_control(id, location, 5.0)?;
-            std::thread::sleep(Duration::from_millis(50));
+            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.read_all_pending_responses()?;
         Ok(())
     };
 
     // Initialize both groups of motors
+    std::thread::sleep(Duration::from_millis(100));
     initialize_motors(&mut motors_1)?;
-    // initialize_motors(&mut motors_2)?;
+    std::thread::sleep(Duration::from_millis(100));
+    initialize_motors(&mut motors_2)?;
 
     println!("Motors initialized and set to stand position.");
 
+    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(50));
-        set_location(&mut motors_1, 0.0)?;
-        // set_location(&mut motors_2, 0.0)?;
+    //     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
     for id in 1..=5 {
         motors_1.send_reset(id)?;
-        // motors_2.send_reset(id)?;
-        std::thread::sleep(Duration::from_millis(50));
+        motors_2.send_reset(id)?;
+        std::thread::sleep(Duration::from_millis(100));
     }
 
     println!("Motors reset. Exiting.");
 
     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
+            }
+        }
+    }
+}

actuator/rust/robstride/src/lib.rs

@@ -436,8 +436,8 @@ impl Motors {
         }
     }
 
-    pub fn send_position_control(&mut self, motor_id: u8, pos_set: f32, kp_set: f32) -> Result<(), std::io::Error> {
-        self.send_motor_control(motor_id, pos_set, 0.0, kp_set, 0.0, 0.0)
+    pub fn send_position_control(&mut self, motor_id: u8, pos_set: f32, kp_set: f32, kd_set: f32) -> Result<(), std::io::Error> {
+        self.send_motor_control(motor_id, pos_set, 0.0, kp_set, kd_set, 0.0)
     }
 
     pub fn send_torque_control(&mut self, motor_id: u8, torque_set: f32) -> Result<(), std::io::Error> {

actuator/rust/robstride/src/main.rs

@@ -18,13 +18,11 @@ fn main() -> Result<(), Box<dyn Error>> {
     })?;
 
     // Create motor instances
-    let motor1 = Motor::new(&ROBSTRIDE_CONFIGS["04"], 1);
-    let motor2 = Motor::new(&ROBSTRIDE_CONFIGS["01"], 2);
+    let motor = Motor::new(&ROBSTRIDE_CONFIGS["04"], 1);
 
     // Insert motors into a HashMap
     let mut motors_map = HashMap::new();
-    motors_map.insert(1, motor1);
-    motors_map.insert(2, motor2);
+    motors_map.insert(1, motor);
 
     // Create a Motors instance with the port name
     let mut motors = Motors::new("/dev/ttyCH341USB0", motors_map)?; // Adjust the device path as needed
@@ -41,26 +39,12 @@ fn main() -> Result<(), Box<dyn Error>> {
 
     motors.read_all_pending_responses()?;
 
-    motors.send_reset(2)?;
-    std::thread::sleep(Duration::from_millis(50));
-    motors.send_set_mode(2, RunMode::MitMode)?;
-    std::thread::sleep(Duration::from_millis(50));
-    motors.send_start(2)?;
-    std::thread::sleep(Duration::from_millis(50));
-    motors.send_set_speed_limit(2, 10.0)?;
-    std::thread::sleep(Duration::from_millis(50));
-    motors.send_set_zero(2)?;
-
-    motors.read_all_pending_responses()?;
-
     let start_time = Instant::now();
     let mut command_count = 0; // Initialize a counter for commands
 
     // PD controller parameters
     let kp_04 = 4.0;
     let kd_04 = 0.1;
-    let kp_01 = 1.0;
-    let kd_01 = 0.1;
 
     // Define period and amplitude
     let period = 2.0;
@@ -71,28 +55,22 @@ fn main() -> Result<(), Box<dyn Error>> {
 
         // Calculate desired positions using a sinusoidal function with specified period and amplitude
         let desired_position_1 = amplitude * (elapsed_time * 2.0 * PI / period).sin();
-        let desired_position_2 = -amplitude * (elapsed_time * 2.0 * PI / period).sin();
 
         // Get current feedback for each motor
         let current_position_1 = motors.get_latest_feedback(1).map_or(0.0, |f| f.position) as f32;
-        let current_position_2 = motors.get_latest_feedback(2).map_or(0.0, |f| f.position) as f32;
 
         // Calculate velocity (derivative of position)
         let current_velocity_1 = motors.get_latest_feedback(1).map_or(0.0, |f| f.velocity) as f32;
-        let current_velocity_2 = motors.get_latest_feedback(2).map_or(0.0, |f| f.velocity) as f32;
 
         // Calculate torque using PD control
         let torque_1 = kp_04 * (desired_position_1 - current_position_1) - kd_04 * current_velocity_1;
-        let torque_2 = kp_01 * (desired_position_2 - current_position_2) - kd_01 * current_velocity_2;
 
         // Send torque commands to the motors
         motors.send_torque_control(1, torque_1 as f32)?;
         std::thread::sleep(Duration::from_millis(4)); // Sleep to prevent overwhelming the bus
-        motors.send_torque_control(2, torque_2 as f32)?;
-        std::thread::sleep(Duration::from_millis(4));
 
         // Increment the command counter
-        command_count += 2; // Two commands sent per loop iteration
+        command_count += 1; // One command sent per loop iteration
 
         // Read feedback from the motors
         motors.read_all_pending_responses()?;
@@ -102,10 +80,6 @@ fn main() -> Result<(), Box<dyn Error>> {
         //     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);
@@ -114,11 +88,10 @@ fn main() -> Result<(), Box<dyn Error>> {
     let elapsed_time = start_time.elapsed().as_secs_f32();
 
     println!("Done");
-    println!("Average control frequency: {:.2} Hz", (command_count as f32 / elapsed_time) / 2.0); // Divide by 2 because two commands are sent per loop
+    println!("Average control frequency: {:.2} Hz", (command_count as f32 / elapsed_time));
 
     // Reset motors on exit
     motors.send_reset(1)?;
-    motors.send_reset(2)?;
 
     Ok(())
 }