profiling script

.gitignore

@@ -23,3 +23,6 @@ out*/
 # Rust
 target/
 Cargo.lock
+
+# CSV files
+*.csv

actuator/rust/README.md

@@ -11,3 +11,15 @@ To run the stub generator:
 ```bash
 cargo run --bin stub_gen
 ```
+
+On Linux, you may need to install libudev-dev for Rust to properly build.
+
+```bash
+sudo apt-get install libudev-dev
+```
+
+To run the profiling script:
+
+```bash
+cargo run --bin profile
+```

actuator/rust/robstride/Cargo.toml

@@ -14,3 +14,4 @@ crate-type = ["cdylib", "rlib"]
 
 [dependencies]
 serialport = "4.5.1"
+ctrlc = "3.4.5"

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

@@ -0,0 +1,65 @@
+use robstride::{Motor, RunMode, ROBSTRIDE01_CONFIG};
+use std::thread;
+use std::time::{Duration, Instant};
+use std::f32::consts::PI;
+use std::fs::File;
+use std::io::{self, Write};
+use std::sync::Arc;
+use std::sync::atomic::{AtomicBool, Ordering};
+use ctrlc;
+
+fn main() -> Result<(), Box<dyn std::error::Error>> {
+    println!("Starting sinusoidal profiling");
+
+    let mut motor = Motor::new("/dev/ttyCH341USB0", ROBSTRIDE01_CONFIG, 1)?;
+
+    motor.send_set_mode(RunMode::PositionMode)?;
+    thread::sleep(Duration::from_millis(50));
+
+    motor.send_reset()?;
+    motor.send_start()?;
+    thread::sleep(Duration::from_millis(50));
+
+    motor.send_set_speed_limit(10.0)?;
+    thread::sleep(Duration::from_millis(50));
+
+    let period = 5.0; // seconds
+    let amplitude = PI / 2.0;
+    let duration = 20.0; // total duration of the profiling in seconds
+    let start_time = Instant::now();
+
+    let mut file = File::create("motor_profile.csv")?;
+    writeln!(file, "Step,Desired Position,Actual Position")?;
+
+    let loop_duration = Duration::from_millis(4); // 250 Hz
+    let mut step = 0;
+
+    let running = Arc::new(AtomicBool::new(true));
+    let r = running.clone();
+
+    ctrlc::set_handler(move || {
+        r.store(false, Ordering::SeqCst);
+    }).expect("Error setting Ctrl-C handler");
+
+    while running.load(Ordering::SeqCst) && start_time.elapsed().as_secs_f32() < duration {
+        let loop_start = Instant::now();
+        let elapsed = start_time.elapsed().as_secs_f32();
+        let desired_position = amplitude * (2.0 * PI * elapsed / period).sin();
+        let feedback = motor.send_set_location(desired_position)?;
+
+        let actual_position = feedback.position;
+
+        writeln!(file, "{},{},{}", step, desired_position, actual_position)?;
+
+        let elapsed_loop = loop_start.elapsed();
+        if elapsed_loop < loop_duration {
+            thread::sleep(loop_duration - elapsed_loop);
+        }
+
+        step += 1;
+    }
+
+    motor.send_reset()?;
+    println!("Sinusoidal profiling finished");
+    Ok(())
+}

actuator/rust/robstride/src/lib.rs

@@ -180,7 +180,7 @@ fn txd_pack(port: &mut Box<dyn SerialPort>, pack: &CanPack) -> Result<(), std::i
     buffer.extend_from_slice(&pack.data[..pack.len as usize]);
     buffer.extend_from_slice(b"\r\n");
 
-    println!("tx {:02X?}", buffer);
+    // println!("tx {:02X?}", buffer);
 
     port.write_all(&buffer)?;
     port.flush()?;
@@ -194,7 +194,7 @@ fn read_bytes(
     let mut buffer = [0u8; 17];
     let bytes_read = port.read(&mut buffer)?;
 
-    println!("rx {:02X?}", &buffer[..bytes_read]);
+    // println!("rx {:02X?}", &buffer[..bytes_read]);
 
     if bytes_read == 17 && buffer[0] == b'A' && buffer[1] == b'T' {
         let addr = u32::from_be_bytes([buffer[2], buffer[3], buffer[4], buffer[5]]) >> 3;
@@ -226,13 +226,13 @@ fn read_bytes(
             faults,
         };
 
-        println!("Parsed data:");
-        println!("  Motor ID: {}", feedback.can_id);
-        println!("  Position: {}", feedback.position);
-        println!("  Velocity: {}", feedback.velocity);
-        println!("  Torque: {}", feedback.torque);
-        println!("  Mode: {:?}", feedback.mode);
-        println!("  Faults: {:?}", feedback.faults);
+        // println!("Parsed data:");
+        // println!("  Motor ID: {}", feedback.can_id);
+        // println!("  Position: {}", feedback.position);
+        // println!("  Velocity: {}", feedback.velocity);
+        // println!("  Torque: {}", feedback.torque);
+        // println!("  Mode: {:?}", feedback.mode);
+        // println!("  Faults: {:?}", feedback.faults);
 
         Ok(feedback)
     } else {

actuator/rust/robstride/src/main.rs

@@ -5,7 +5,7 @@ use std::time::Duration;
 fn main() -> Result<(), Box<dyn std::error::Error>> {
     println!("Starting program");
 
-    let mut motor = Motor::new("/dev/ttyUSB0", ROBSTRIDE01_CONFIG, 2)?;
+    let mut motor = Motor::new("/dev/ttyCH341USB0", ROBSTRIDE01_CONFIG, 1)?;
 
     motor.send_set_mode(RunMode::PositionMode)?;
     thread::sleep(Duration::from_millis(50));