From 0f09f985e07c897654b7a0d526fa61733579e2ab Mon Sep 17 00:00:00 2001 From: WT-MM Date: Mon, 7 Oct 2024 22:17:39 -0700 Subject: [PATCH 01/10] profiling script --- .gitignore | 3 + actuator/rust/README.md | 12 ++++ actuator/rust/robstride/Cargo.toml | 1 + actuator/rust/robstride/src/bin/profile.rs | 65 ++++++++++++++++++++++ actuator/rust/robstride/src/lib.rs | 18 +++--- actuator/rust/robstride/src/main.rs | 2 +- 6 files changed, 91 insertions(+), 10 deletions(-) create mode 100644 actuator/rust/robstride/src/bin/profile.rs diff --git a/.gitignore b/.gitignore index 38f9946..88e541b 100644 --- a/.gitignore +++ b/.gitignore @@ -23,3 +23,6 @@ out*/ # Rust target/ Cargo.lock + +# CSV files +*.csv diff --git a/actuator/rust/README.md b/actuator/rust/README.md index 7a972d3..02c6a27 100644 --- a/actuator/rust/README.md +++ b/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 +``` diff --git a/actuator/rust/robstride/Cargo.toml b/actuator/rust/robstride/Cargo.toml index a45b232..bf238cd 100644 --- a/actuator/rust/robstride/Cargo.toml +++ b/actuator/rust/robstride/Cargo.toml @@ -14,3 +14,4 @@ crate-type = ["cdylib", "rlib"] [dependencies] serialport = "4.5.1" +ctrlc = "3.4.5" diff --git a/actuator/rust/robstride/src/bin/profile.rs b/actuator/rust/robstride/src/bin/profile.rs new file mode 100644 index 0000000..bcf6641 --- /dev/null +++ b/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> { + 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(()) +} diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index 6228d71..a6ef493 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -180,7 +180,7 @@ fn txd_pack(port: &mut Box, 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 { diff --git a/actuator/rust/robstride/src/main.rs b/actuator/rust/robstride/src/main.rs index caede6a..09394bd 100644 --- a/actuator/rust/robstride/src/main.rs +++ b/actuator/rust/robstride/src/main.rs @@ -5,7 +5,7 @@ use std::time::Duration; fn main() -> Result<(), Box> { 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)); From d600ae16ce7438185fa4fcae6f140f9ad5372cbe Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 8 Oct 2024 12:21:31 -0700 Subject: [PATCH 02/10] fix profiling for refactor --- actuator/rust/robstride/src/bin/profile.rs | 28 +++++++++++---- actuator/rust/robstride/src/lib.rs | 40 ++++++++++++++-------- 2 files changed, 47 insertions(+), 21 deletions(-) diff --git a/actuator/rust/robstride/src/bin/profile.rs b/actuator/rust/robstride/src/bin/profile.rs index bcf6641..f04f8b0 100644 --- a/actuator/rust/robstride/src/bin/profile.rs +++ b/actuator/rust/robstride/src/bin/profile.rs @@ -1,4 +1,4 @@ -use robstride::{Motor, RunMode, ROBSTRIDE01_CONFIG}; +use robstride::{Motor, RunMode, ROBSTRIDE_CONFIGS}; use std::thread; use std::time::{Duration, Instant}; use std::f32::consts::PI; @@ -8,12 +8,20 @@ use std::sync::Arc; use std::sync::atomic::{AtomicBool, Ordering}; use ctrlc; +fn calculate_torque(desired_position: f32, actual_position: f32, kp: f32, kd: f32, actual_velocity: f32) -> f32 { + let position_error = desired_position - actual_position; + let velocity_error = -actual_velocity; + kp * position_error + kd * velocity_error +} + fn main() -> Result<(), Box> { println!("Starting sinusoidal profiling"); - let mut motor = Motor::new("/dev/ttyCH341USB0", ROBSTRIDE01_CONFIG, 1)?; + // Use the updated MotorConfig from ROBSTRIDE_CONFIGS + let motor_config = ROBSTRIDE_CONFIGS.get("01").expect("Config not found"); + let mut motor = Motor::new("/dev/ttyCH341USB0", motor_config, 1)?; - motor.send_set_mode(RunMode::PositionMode)?; + motor.send_set_mode(RunMode::MitMode)?; thread::sleep(Duration::from_millis(50)); motor.send_reset()?; @@ -45,11 +53,17 @@ fn main() -> Result<(), Box> { 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; + // Read feedback from the motor + let responses = motor.read_all_pending_responses()?; + let feedback = responses.get(0).unwrap(); + + let torque = calculate_torque(desired_position, feedback.position, 5.0, 0.1, feedback.velocity); + + writeln!(file, "{},{},{}", step, desired_position, feedback.position)?; + println!("{},{},{},{}", step, desired_position, feedback.position, torque); - writeln!(file, "{},{},{}", step, desired_position, actual_position)?; + motor.send_torque_control(torque)?; let elapsed_loop = loop_start.elapsed(); if elapsed_loop < loop_duration { @@ -60,6 +74,6 @@ fn main() -> Result<(), Box> { } motor.send_reset()?; - println!("Sinusoidal profiling finished"); + println!("Sinusoidal profiling finished. Closed cleanly."); Ok(()) } diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index 1a67409..10c1c62 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -108,8 +108,9 @@ pub enum CanComMode { } #[repr(u8)] -#[derive(Copy, Clone, Debug)] +#[derive(Copy, Clone, Debug, Default)] pub enum MotorMode { + #[default] Reset = 0, Cali, Motor, @@ -140,6 +141,7 @@ pub struct CanPack { pub data: [u8; 8], } +#[derive(Debug, Default)] pub struct MotorFeedback { pub can_id: u16, pub position: f32, @@ -267,7 +269,7 @@ fn read_bytes( } } -struct Motor { +pub struct Motor { port: Box, config: &'static MotorConfig, id: u8, @@ -295,7 +297,7 @@ impl Motor { Ok(()) } - fn send_set_mode(&mut self, run_mode: RunMode) -> Result<(), std::io::Error> { + pub fn send_set_mode(&mut self, run_mode: RunMode) -> Result<(), std::io::Error> { let mut pack = CanPack { ex_id: ExId { id: self.id, @@ -315,7 +317,7 @@ impl Motor { Ok(()) } - fn send_reset(&mut self) -> Result<(), std::io::Error> { + pub fn send_reset(&mut self) -> Result<(), std::io::Error> { let pack = CanPack { ex_id: ExId { id: self.id, @@ -330,7 +332,7 @@ impl Motor { self.send_command(&pack) } - fn send_start(&mut self) -> Result<(), std::io::Error> { + pub fn send_start(&mut self) -> Result<(), std::io::Error> { let pack = CanPack { ex_id: ExId { id: self.id, @@ -345,7 +347,7 @@ impl Motor { self.send_command(&pack) } - fn send_set_speed_limit(&mut self, speed: f32) -> Result<(), std::io::Error> { + pub fn send_set_speed_limit(&mut self, speed: f32) -> Result<(), std::io::Error> { let mut pack = CanPack { ex_id: ExId { id: self.id, @@ -364,7 +366,7 @@ impl Motor { self.send_command(&pack) } - fn send_set_location(&mut self, location: f32) -> Result<(), std::io::Error> { + pub fn send_set_location(&mut self, location: f32) -> Result<(), std::io::Error> { let mut pack = CanPack { ex_id: ExId { id: self.id, @@ -422,7 +424,7 @@ impl Motor { self.send_command(&pack) } - fn read_all_pending_responses(&mut self) -> Result, std::io::Error> { + pub fn read_all_pending_responses(&mut self) -> Result, std::io::Error> { let mut feedbacks = Vec::new(); while self.pending_responses > 0 { match read_bytes(&mut self.port, self.config) { @@ -440,6 +442,16 @@ impl Motor { } Ok(feedbacks) } + + // Note that you need to set the RunMode to MitMode before calling this + pub fn send_position_control(&mut self, pos_set: f32, kp_set: f32) -> Result<(), std::io::Error> { + self.send_motor_control(pos_set, 0.0, kp_set, 0.0, 0.0) + } + + // Note that you need to set the RunMode to MitMode before calling this + pub fn send_torque_control(&mut self, torque_set: f32) -> Result<(), std::io::Error> { + self.send_motor_control(0.0, 0.0, 0.0, 0.0, torque_set) + } } // Add this helper function outside of the Motor impl block @@ -521,11 +533,11 @@ impl Motors { Ok(feedbacks) } - fn send_position_control(&mut self, pos_set: f32, kp_set: f32) -> Result<(), std::io::Error> { - self.send_motor_control(pos_set, 0.0, kp_set, 0.0, 0.0) - } + // fn send_position_control(&mut self, pos_set: f32, kp_set: f32) -> Result<(), std::io::Error> { + // self.send_motor_control(pos_set, 0.0, kp_set, 0.0, 0.0) + // } - fn send_torque_control(&mut self, torque_set: f32) -> Result<(), std::io::Error> { - self.send_motor_control(0.0, 0.0, 0.0, 0.0, torque_set) - } + // fn send_torque_control(&mut self, torque_set: f32) -> Result<(), std::io::Error> { + // self.send_motor_control(0.0, 0.0, 0.0, 0.0, torque_set) + // } } From 2ea3bc6e525352ed02aa0adefda4e052431d695c Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 8 Oct 2024 14:40:04 -0700 Subject: [PATCH 03/10] add zeroing --- actuator/rust/robstride/Cargo.toml | 1 + actuator/rust/robstride/src/bin/profile.rs | 14 +++++++-- actuator/rust/robstride/src/lib.rs | 15 +++++++++ actuator/rust/robstride/src/main.rs | 36 +++++++++++----------- 4 files changed, 45 insertions(+), 21 deletions(-) diff --git a/actuator/rust/robstride/Cargo.toml b/actuator/rust/robstride/Cargo.toml index 3823128..8a93fb3 100644 --- a/actuator/rust/robstride/Cargo.toml +++ b/actuator/rust/robstride/Cargo.toml @@ -16,3 +16,4 @@ crate-type = ["cdylib", "rlib"] serialport = "4.5.1" ctrlc = "3.4.5" lazy_static = "1.4.0" +spin_sleep = "1.2.1" diff --git a/actuator/rust/robstride/src/bin/profile.rs b/actuator/rust/robstride/src/bin/profile.rs index f04f8b0..5ea34aa 100644 --- a/actuator/rust/robstride/src/bin/profile.rs +++ b/actuator/rust/robstride/src/bin/profile.rs @@ -7,6 +7,7 @@ use std::io::{self, Write}; use std::sync::Arc; use std::sync::atomic::{AtomicBool, Ordering}; use ctrlc; +use spin_sleep::SpinSleeper; fn calculate_torque(desired_position: f32, actual_position: f32, kp: f32, kd: f32, actual_velocity: f32) -> f32 { let position_error = desired_position - actual_position; @@ -31,6 +32,9 @@ fn main() -> Result<(), Box> { motor.send_set_speed_limit(10.0)?; thread::sleep(Duration::from_millis(50)); + motor.send_set_zero()?; + 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 @@ -39,7 +43,8 @@ fn main() -> Result<(), Box> { 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 loop_duration = Duration::from_millis(4); // 250 Hz + let loop_duration = Duration::from_millis(5); // 200 Hz let mut step = 0; let running = Arc::new(AtomicBool::new(true)); @@ -49,6 +54,8 @@ fn main() -> Result<(), Box> { r.store(false, Ordering::SeqCst); }).expect("Error setting Ctrl-C handler"); + let sleeper = SpinSleeper::new(1000).with_spin_strategy(spin_sleep::SpinStrategy::YieldThread); + 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(); @@ -57,17 +64,18 @@ fn main() -> Result<(), Box> { // Read feedback from the motor let responses = motor.read_all_pending_responses()?; let feedback = responses.get(0).unwrap(); + println!("{:?}", feedback); let torque = calculate_torque(desired_position, feedback.position, 5.0, 0.1, feedback.velocity); writeln!(file, "{},{},{}", step, desired_position, feedback.position)?; - println!("{},{},{},{}", step, desired_position, feedback.position, torque); + // println!("{},{},{},{}", step, desired_position, feedback.position, torque); motor.send_torque_control(torque)?; let elapsed_loop = loop_start.elapsed(); if elapsed_loop < loop_duration { - thread::sleep(loop_duration - elapsed_loop); + sleeper.sleep(loop_duration - elapsed_loop); } step += 1; diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index 10c1c62..799bdae 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -347,6 +347,21 @@ impl Motor { self.send_command(&pack) } + pub fn send_set_zero(&mut self) -> Result<(), std::io::Error> { + let pack = CanPack { + ex_id: ExId { + id: self.id, + data: CAN_ID_DEBUG_UI as u16, + mode: CanComMode::MotorZero, + res: 0, + }, + len: 8, + data: [1, 0, 0, 0, 0, 0, 0, 0], // Set first byte to 1 as per documentation + }; + + self.send_command(&pack) + } + pub fn send_set_speed_limit(&mut self, speed: f32) -> Result<(), std::io::Error> { let mut pack = CanPack { ex_id: ExId { diff --git a/actuator/rust/robstride/src/main.rs b/actuator/rust/robstride/src/main.rs index 6e51d8a..1a586cc 100644 --- a/actuator/rust/robstride/src/main.rs +++ b/actuator/rust/robstride/src/main.rs @@ -3,31 +3,31 @@ use std::thread; use std::time::Duration; fn main() -> Result<(), Box> { - println!("Starting program"); + // println!("Starting program"); - let motors = Motors::new("/dev/ttyCH341USB0", &[(&ROBSTRIDE_CONFIGS["01"], 2)])?; + // let motors = Motors::new("/dev/ttyCH341USB0", &[(&ROBSTRIDE_CONFIGS["01"], 2)])?; - motors.send_set_mode(RunMode::PositionMode)?; - thread::sleep(Duration::from_millis(50)); + // motors.send_set_mode(RunMode::PositionMode)?; + // thread::sleep(Duration::from_millis(50)); - motors.send_reset()?; - motors.send_start()?; - thread::sleep(Duration::from_millis(50)); + // motors.send_reset()?; + // motors.send_start()?; + // thread::sleep(Duration::from_millis(50)); - motors.send_set_speed_limit(5.0)?; - thread::sleep(Duration::from_millis(50)); + // motors.send_set_speed_limit(5.0)?; + // thread::sleep(Duration::from_millis(50)); - for i in 0..3 { - motors.send_set_location(std::f32::consts::PI * i as f32 / 2.0)?; - thread::sleep(Duration::from_secs(1)); - } + // for i in 0..3 { + // motors.send_set_location(std::f32::consts::PI * i as f32 / 2.0)?; + // thread::sleep(Duration::from_secs(1)); + // } - motors.send_set_speed_limit(10.0)?; - motors.send_set_location(0.0)?; - thread::sleep(Duration::from_secs(2)); + // motors.send_set_speed_limit(10.0)?; + // motors.send_set_location(0.0)?; + // thread::sleep(Duration::from_secs(2)); - motors.send_reset()?; + // motors.send_reset()?; - println!("Program finished"); + // println!("Program finished"); Ok(()) } From fbc554db9fc908bb6f1e9a71a21a35911fc609cc Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 8 Oct 2024 17:51:06 -0700 Subject: [PATCH 04/10] new Motors implementation --- actuator/rust/robstride/src/bin/profile.rs | 96 +++---- actuator/rust/robstride/src/lib.rs | 307 ++++++++------------- actuator/rust/robstride/src/main.rs | 129 +++++++-- 3 files changed, 273 insertions(+), 259 deletions(-) diff --git a/actuator/rust/robstride/src/bin/profile.rs b/actuator/rust/robstride/src/bin/profile.rs index 5ea34aa..870a8d1 100644 --- a/actuator/rust/robstride/src/bin/profile.rs +++ b/actuator/rust/robstride/src/bin/profile.rs @@ -16,72 +16,72 @@ fn calculate_torque(desired_position: f32, actual_position: f32, kp: f32, kd: f3 } fn main() -> Result<(), Box> { - println!("Starting sinusoidal profiling"); + // println!("Starting sinusoidal profiling"); - // Use the updated MotorConfig from ROBSTRIDE_CONFIGS - let motor_config = ROBSTRIDE_CONFIGS.get("01").expect("Config not found"); - let mut motor = Motor::new("/dev/ttyCH341USB0", motor_config, 1)?; + // // Use the updated MotorConfig from ROBSTRIDE_CONFIGS + // let motor_config = ROBSTRIDE_CONFIGS.get("01").expect("Config not found"); + // let mut motor = Motor::new("/dev/ttyCH341USB0", motor_config, 1)?; - motor.send_set_mode(RunMode::MitMode)?; - thread::sleep(Duration::from_millis(50)); + // motor.send_set_mode(RunMode::MitMode)?; + // thread::sleep(Duration::from_millis(50)); - motor.send_reset()?; - motor.send_start()?; - 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)); + // motor.send_set_speed_limit(10.0)?; + // thread::sleep(Duration::from_millis(50)); - motor.send_set_zero()?; - thread::sleep(Duration::from_millis(50)); + // motor.send_set_zero()?; + // 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 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 mut file = File::create("motor_profile.csv")?; + // writeln!(file, "Step,Desired Position,Actual Position")?; - // let loop_duration = Duration::from_millis(4); // 250 Hz - let loop_duration = Duration::from_millis(5); // 200 Hz - let mut step = 0; + // // let loop_duration = Duration::from_millis(4); // 250 Hz + // let loop_duration = Duration::from_millis(5); // 200 Hz + // let mut step = 0; - let running = Arc::new(AtomicBool::new(true)); - let r = running.clone(); + // 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"); + // ctrlc::set_handler(move || { + // r.store(false, Ordering::SeqCst); + // }).expect("Error setting Ctrl-C handler"); - let sleeper = SpinSleeper::new(1000).with_spin_strategy(spin_sleep::SpinStrategy::YieldThread); + // let sleeper = SpinSleeper::new(1000).with_spin_strategy(spin_sleep::SpinStrategy::YieldThread); - 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(); + // 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(); - // Read feedback from the motor - let responses = motor.read_all_pending_responses()?; - let feedback = responses.get(0).unwrap(); - println!("{:?}", feedback); + // // Read feedback from the motor + // let responses = motor.read_all_pending_responses()?; + // let feedback = responses.get(0).unwrap(); + // println!("{:?}", feedback); - let torque = calculate_torque(desired_position, feedback.position, 5.0, 0.1, feedback.velocity); + // let torque = calculate_torque(desired_position, feedback.position, 5.0, 0.1, feedback.velocity); - writeln!(file, "{},{},{}", step, desired_position, feedback.position)?; - // println!("{},{},{},{}", step, desired_position, feedback.position, torque); + // writeln!(file, "{},{},{}", step, desired_position, feedback.position)?; + // // println!("{},{},{},{}", step, desired_position, feedback.position, torque); - motor.send_torque_control(torque)?; + // motor.send_torque_control(torque)?; - let elapsed_loop = loop_start.elapsed(); - if elapsed_loop < loop_duration { - sleeper.sleep(loop_duration - elapsed_loop); - } + // let elapsed_loop = loop_start.elapsed(); + // if elapsed_loop < loop_duration { + // sleeper.sleep(loop_duration - elapsed_loop); + // } - step += 1; - } + // step += 1; + // } - motor.send_reset()?; - println!("Sinusoidal profiling finished. Closed cleanly."); + // motor.send_reset()?; + // println!("Sinusoidal profiling finished. Closed cleanly."); Ok(()) } diff --git a/actuator/rust/robstride/src/lib.rs b/actuator/rust/robstride/src/lib.rs index 799bdae..2487917 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/actuator/rust/robstride/src/lib.rs @@ -1,7 +1,6 @@ use serialport::SerialPort; use std::collections::HashMap; use std::io::{Read, Write}; -use std::thread; use std::time::Duration; #[macro_use] @@ -141,9 +140,9 @@ pub struct CanPack { pub data: [u8; 8], } -#[derive(Debug, Default)] +#[derive(Debug, Default, Clone)] pub struct MotorFeedback { - pub can_id: u16, + pub can_id: u8, pub position: f32, pub velocity: f32, pub torque: f32, @@ -151,6 +150,16 @@ pub struct MotorFeedback { pub faults: u16, } +#[derive(Debug, Default, Clone)] +pub struct MotorFeedbackRaw { + pub can_id: u8, + pub pos_int: u16, + pub vel_int: u16, + pub torque_int: u16, + pub mode: MotorMode, + pub faults: u16, +} + fn init_serial_port(device: &str) -> Result, serialport::Error> { let port = serialport::new(device, BAUDRATE) .data_bits(serialport::DataBits::Eight) @@ -209,15 +218,10 @@ fn txd_pack(port: &mut Box, pack: &CanPack) -> Result<(), std::i Ok(()) } -fn read_bytes( - port: &mut Box, - config: &MotorConfig, -) -> Result { +fn read_bytes(port: &mut Box) -> Result { let mut buffer = [0u8; 17]; let bytes_read = port.read(&mut buffer)?; - // 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; let ex_id = ExId { @@ -227,66 +231,55 @@ fn read_bytes( res: 0, }; - let can_id = ex_id.data & 0x00FF; + let can_id = (ex_id.data & 0x00FF) as u8; let faults = (ex_id.data & 0x3F00) >> 8; let mode = unsafe { std::mem::transmute(((ex_id.data & 0xC000) >> 14) as u8) }; - let pos_int_get = u16::from_be_bytes([buffer[7], buffer[8]]); - let vel_int_get = u16::from_be_bytes([buffer[9], buffer[10]]); - let torque_int_get = u16::from_be_bytes([buffer[11], buffer[12]]); - - let position = uint_to_float(pos_int_get, config.p_min, config.p_max, 16); - let velocity = uint_to_float(vel_int_get, config.v_min, config.v_max, 16); - let torque = uint_to_float(torque_int_get, config.t_min, config.t_max, 16); + let pos_int = u16::from_be_bytes([buffer[7], buffer[8]]); + let vel_int = u16::from_be_bytes([buffer[9], buffer[10]]); + let torque_int = u16::from_be_bytes([buffer[11], buffer[12]]); - let feedback = MotorFeedback { + Ok(MotorFeedbackRaw { can_id, - position, - velocity, - torque, + pos_int, + vel_int, + torque_int, mode, 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 { - Ok(MotorFeedback { - can_id: 0, - position: 0.0, - velocity: 0.0, - torque: 0.0, - mode: MotorMode::Reset, - faults: 0, }) + } else { + Ok(MotorFeedbackRaw::default()) } } pub struct Motor { - port: Box, config: &'static MotorConfig, id: u8, - pending_responses: usize, } impl Motor { + pub fn new(config: &'static MotorConfig, id: u8) -> Self { + Motor { config, id } + } +} + +pub struct Motors { + port: Box, + motors: HashMap, + latest_feedback: HashMap, + pending_responses: usize, +} + +impl Motors { pub fn new( - tty_port: &str, - config: &'static MotorConfig, - id: u8, + port_name: &str, + motors: HashMap, ) -> Result> { - let port = init_serial_port(tty_port)?; - Ok(Motor { + let port = init_serial_port(port_name)?; + Ok(Motors { port, - config, - id, + motors, + latest_feedback: HashMap::new(), pending_responses: 0, }) } @@ -297,10 +290,10 @@ impl Motor { Ok(()) } - pub fn send_set_mode(&mut self, run_mode: RunMode) -> Result<(), std::io::Error> { + pub fn send_set_mode(&mut self, motor_id: u8, mode: RunMode) -> Result<(), std::io::Error> { let mut pack = CanPack { ex_id: ExId { - id: self.id, + id: motor_id, data: CAN_ID_DEBUG_UI as u16, mode: CanComMode::SdoWrite, res: 0, @@ -311,16 +304,15 @@ impl Motor { let index: u16 = 0x7005; pack.data[..2].copy_from_slice(&index.to_le_bytes()); - pack.data[4] = run_mode as u8; + pack.data[4] = mode as u8; - self.send_command(&pack)?; - Ok(()) + self.send_command(&pack) } - pub fn send_reset(&mut self) -> Result<(), std::io::Error> { + pub fn send_reset(&mut self, motor_id: u8) -> Result<(), std::io::Error> { let pack = CanPack { ex_id: ExId { - id: self.id, + id: motor_id, data: CAN_ID_DEBUG_UI as u16, mode: CanComMode::MotorReset, res: 0, @@ -332,10 +324,10 @@ impl Motor { self.send_command(&pack) } - pub fn send_start(&mut self) -> Result<(), std::io::Error> { + pub fn send_start(&mut self, motor_id: u8) -> Result<(), std::io::Error> { let pack = CanPack { ex_id: ExId { - id: self.id, + id: motor_id, data: CAN_ID_DEBUG_UI as u16, mode: CanComMode::MotorIn, res: 0, @@ -347,10 +339,10 @@ impl Motor { self.send_command(&pack) } - pub fn send_set_zero(&mut self) -> Result<(), std::io::Error> { + pub fn send_set_zero(&mut self, motor_id: u8) -> Result<(), std::io::Error> { let pack = CanPack { ex_id: ExId { - id: self.id, + id: motor_id, data: CAN_ID_DEBUG_UI as u16, mode: CanComMode::MotorZero, res: 0, @@ -362,10 +354,10 @@ impl Motor { self.send_command(&pack) } - pub fn send_set_speed_limit(&mut self, speed: f32) -> Result<(), std::io::Error> { + pub fn send_set_speed_limit(&mut self, motor_id: u8, speed: f32) -> Result<(), std::io::Error> { let mut pack = CanPack { ex_id: ExId { - id: self.id, + id: motor_id, data: CAN_ID_DEBUG_UI as u16, mode: CanComMode::SdoWrite, res: 0, @@ -381,10 +373,10 @@ impl Motor { self.send_command(&pack) } - pub fn send_set_location(&mut self, location: f32) -> Result<(), std::io::Error> { + pub fn send_set_location(&mut self, motor_id: u8, location: f32) -> Result<(), std::io::Error> { let mut pack = CanPack { ex_id: ExId { - id: self.id, + id: motor_id, data: CAN_ID_DEBUG_UI as u16, mode: CanComMode::SdoWrite, res: 0, @@ -402,157 +394,94 @@ impl Motor { fn send_motor_control( &mut self, + motor_id: u8, pos_set: f32, vel_set: f32, kp_set: f32, kd_set: f32, torque_set: f32, ) -> Result<(), std::io::Error> { - let mut pack = CanPack { - ex_id: ExId { - id: self.id, - data: 0, - mode: CanComMode::MotorCtrl, - res: 0, - }, - len: 8, - data: [0; 8], - }; - - let pos_int_set = float_to_uint(pos_set, self.config.p_min, self.config.p_max, 16); - let vel_int_set = float_to_uint(vel_set, self.config.v_min, self.config.v_max, 16); - let kp_int_set = float_to_uint(kp_set, self.config.kp_min, self.config.kp_max, 16); - let kd_int_set = float_to_uint(kd_set, self.config.kd_min, self.config.kd_max, 16); - let torque_int_set = float_to_uint(torque_set, self.config.t_min, self.config.t_max, 16); - - pack.ex_id.data = torque_int_set; + if let Some(motor) = self.motors.get(&motor_id) { + let mut pack = CanPack { + ex_id: ExId { + id: motor_id as u8, + data: 0, + mode: CanComMode::MotorCtrl, + res: 0, + }, + len: 8, + data: [0; 8], + }; + + let pos_int_set = float_to_uint(pos_set, motor.config.p_min, motor.config.p_max, 16); + let vel_int_set = float_to_uint(vel_set, motor.config.v_min, motor.config.v_max, 16); + let kp_int_set = float_to_uint(kp_set, motor.config.kp_min, motor.config.kp_max, 16); + let kd_int_set = float_to_uint(kd_set, motor.config.kd_min, motor.config.kd_max, 16); + let torque_int_set = float_to_uint(torque_set, motor.config.t_min, motor.config.t_max, 16); + + pack.ex_id.data = torque_int_set; + + pack.data[0] = (pos_int_set >> 8) as u8; + pack.data[1] = (pos_int_set & 0xFF) as u8; + pack.data[2] = (vel_int_set >> 8) as u8; + pack.data[3] = (vel_int_set & 0xFF) as u8; + pack.data[4] = (kp_int_set >> 8) as u8; + pack.data[5] = (kp_int_set & 0xFF) as u8; + pack.data[6] = (kd_int_set >> 8) as u8; + pack.data[7] = (kd_int_set & 0xFF) as u8; + + self.send_command(&pack) + } else { + Err(std::io::Error::new(std::io::ErrorKind::NotFound, "Motor not found")) + } + } - pack.data[0] = (pos_int_set >> 8) as u8; - pack.data[1] = (pos_int_set & 0xFF) as u8; - pack.data[2] = (vel_int_set >> 8) as u8; - pack.data[3] = (vel_int_set & 0xFF) as u8; - pack.data[4] = (kp_int_set >> 8) as u8; - pack.data[5] = (kp_int_set & 0xFF) as u8; - pack.data[6] = (kd_int_set >> 8) as u8; - pack.data[7] = (kd_int_set & 0xFF) as u8; + 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) + } - self.send_command(&pack) + pub fn send_torque_control(&mut self, motor_id: u8, torque_set: f32) -> Result<(), std::io::Error> { + self.send_motor_control(motor_id, 0.0, 0.0, 0.0, 0.0, torque_set) } - pub fn read_all_pending_responses(&mut self) -> Result, std::io::Error> { - let mut feedbacks = Vec::new(); + pub fn read_all_pending_responses(&mut self) -> Result, std::io::Error> { while self.pending_responses > 0 { - match read_bytes(&mut self.port, self.config) { - Ok(feedback) => { - feedbacks.push(feedback); + match read_bytes(&mut self.port) { + Ok(raw_feedback) => { + if let Some(motor) = self.motors.get(&raw_feedback.can_id) { + let position = uint_to_float(raw_feedback.pos_int, motor.config.p_min, motor.config.p_max, 16); + let velocity = uint_to_float(raw_feedback.vel_int, motor.config.v_min, motor.config.v_max, 16); + let torque = uint_to_float(raw_feedback.torque_int, motor.config.t_min, motor.config.t_max, 16); + + let feedback = MotorFeedback { + can_id: raw_feedback.can_id, + position, + velocity, + torque, + mode: raw_feedback.mode, + faults: raw_feedback.faults, + }; + + self.latest_feedback.insert(motor.id, feedback); + } self.pending_responses -= 1; } Err(e) => { - // If there's an error, we'll still decrement pending_responses - // to avoid getting stuck in an infinite loop self.pending_responses -= 1; eprintln!("Error reading response: {:?}", e); } } } - Ok(feedbacks) - } - - // Note that you need to set the RunMode to MitMode before calling this - pub fn send_position_control(&mut self, pos_set: f32, kp_set: f32) -> Result<(), std::io::Error> { - self.send_motor_control(pos_set, 0.0, kp_set, 0.0, 0.0) + Ok(self.latest_feedback.clone()) } - // Note that you need to set the RunMode to MitMode before calling this - pub fn send_torque_control(&mut self, torque_set: f32) -> Result<(), std::io::Error> { - self.send_motor_control(0.0, 0.0, 0.0, 0.0, torque_set) + pub fn get_latest_feedback(&self, motor_id: u8) -> Option<&MotorFeedback> { + self.latest_feedback.get(&motor_id) } } -// Add this helper function outside of the Motor impl block fn float_to_uint(x: f32, x_min: f32, x_max: f32, bits: u8) -> u16 { let span = x_max - x_min; let offset = x_min; ((x - offset) * ((1 << bits) - 1) as f32 / span) as u16 } - -pub struct Motors { - motors: Vec, - current_mode: RunMode, -} - -impl Motors { - pub fn new( - tty_port: &str, - motor_configs: &[(&'static MotorConfig, u8)], - ) -> Result> { - let motors = motor_configs - .iter() - .map(|(config, id)| Motor::new(tty_port, config, *id)) - .collect::, Box>>()?; - - Ok(Motors { - motors, - current_mode: RunMode::UnsetMode, - }) - } - - pub fn send_set_mode(&mut self, run_mode: RunMode) -> Result<(), std::io::Error> { - if self.current_mode == run_mode { - return Ok(()); - } - for motor in &mut self.motors { - motor.send_set_mode(run_mode)?; - } - self.current_mode = run_mode; - Ok(()) - } - - pub fn send_set_locations( - &mut self, - locations: &HashMap, - ) -> Result<(), std::io::Error> { - self.send_set_mode(RunMode::PositionMode)?; - - for motor in &mut self.motors { - if let Some(&location) = locations.get(&motor.id) { - motor.send_set_location(location)?; - } - } - Ok(()) - } - - pub fn send_set_speed_limits( - &mut self, - speed_limits: &HashMap, - ) -> Result<(), std::io::Error> { - for motor in &mut self.motors { - if let Some(&speed_limit) = speed_limits.get(&motor.id) { - motor.send_set_speed_limit(speed_limit)?; - } - } - // Sleep - thread::sleep(Duration::from_millis(50)); - Ok(()) - } - - pub fn read_all_pending_responses( - &mut self, - ) -> Result, std::io::Error> { - let mut feedbacks = HashMap::new(); - for motor in &mut self.motors { - for feedback in motor.read_all_pending_responses()? { - feedbacks.insert(feedback.can_id, feedback); - } - } - Ok(feedbacks) - } - - // fn send_position_control(&mut self, pos_set: f32, kp_set: f32) -> Result<(), std::io::Error> { - // self.send_motor_control(pos_set, 0.0, kp_set, 0.0, 0.0) - // } - - // fn send_torque_control(&mut self, torque_set: f32) -> Result<(), std::io::Error> { - // self.send_motor_control(0.0, 0.0, 0.0, 0.0, torque_set) - // } -} diff --git a/actuator/rust/robstride/src/main.rs b/actuator/rust/robstride/src/main.rs index 1a586cc..f4189e2 100644 --- a/actuator/rust/robstride/src/main.rs +++ b/actuator/rust/robstride/src/main.rs @@ -1,33 +1,118 @@ -use robstride::{Motors, RunMode, ROBSTRIDE_CONFIGS}; -use std::thread; -use std::time::Duration; +use robstride::{Motor, Motors, RunMode, ROBSTRIDE_CONFIGS}; +use std::collections::HashMap; +use std::error::Error; +use std::f64::consts::PI; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::Arc; +use std::time::{Duration, Instant}; +use ctrlc; -fn main() -> Result<(), Box> { - // println!("Starting program"); +fn main() -> Result<(), Box> { + // Create an atomic flag to handle SIGINT + let running = Arc::new(AtomicBool::new(true)); + let r = running.clone(); - // let motors = Motors::new("/dev/ttyCH341USB0", &[(&ROBSTRIDE_CONFIGS["01"], 2)])?; + // Set up the SIGINT handler + ctrlc::set_handler(move || { + r.store(false, Ordering::SeqCst); + })?; - // motors.send_set_mode(RunMode::PositionMode)?; - // thread::sleep(Duration::from_millis(50)); + // Create motor instances + let motor1 = Motor::new(&ROBSTRIDE_CONFIGS["04"], 1); + let motor2 = Motor::new(&ROBSTRIDE_CONFIGS["01"], 2); - // motors.send_reset()?; - // motors.send_start()?; - // thread::sleep(Duration::from_millis(50)); + // Insert motors into a HashMap + let mut motors_map = HashMap::new(); + motors_map.insert(1, motor1); + motors_map.insert(2, motor2); - // motors.send_set_speed_limit(5.0)?; - // thread::sleep(Duration::from_millis(50)); + // Create a Motors instance with the port name + let mut motors = Motors::new("/dev/ttyCH341USB0", motors_map)?; // Adjust the device path as needed - // for i in 0..3 { - // motors.send_set_location(std::f32::consts::PI * i as f32 / 2.0)?; - // thread::sleep(Duration::from_secs(1)); - // } + motors.send_reset(1)?; + std::thread::sleep(Duration::from_millis(50)); + motors.send_set_mode(1, RunMode::MitMode)?; + std::thread::sleep(Duration::from_millis(50)); + motors.send_start(1)?; + std::thread::sleep(Duration::from_millis(50)); + motors.send_set_speed_limit(1, 10.0)?; + std::thread::sleep(Duration::from_millis(50)); + motors.send_set_zero(1)?; - // motors.send_set_speed_limit(10.0)?; - // motors.send_set_location(0.0)?; - // thread::sleep(Duration::from_secs(2)); + motors.read_all_pending_responses()?; - // motors.send_reset()?; + 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 = 3.0; + let kd = 0.0; + + // Define period and amplitude + let period = 10.0; + let amplitude = PI / 2.0; + + while running.load(Ordering::SeqCst) && start_time.elapsed() < Duration::new(10, 0) { + let elapsed_time = start_time.elapsed().as_secs_f64(); + + // 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 f64; + let current_position_2 = motors.get_latest_feedback(2).map_or(0.0, |f| f.position) as f64; + + // Calculate velocity (derivative of position) + let current_velocity_1 = motors.get_latest_feedback(1).map_or(0.0, |f| f.velocity) as f64; + let current_velocity_2 = motors.get_latest_feedback(2).map_or(0.0, |f| f.velocity) as f64; + + // Calculate torque using PD control + let torque_1 = kp * (desired_position_1 - current_position_1) - kd * current_velocity_1; + let torque_2 = kp * (desired_position_2 - current_position_2) - kd * current_velocity_2; + + // Send torque commands to the motors + motors.send_torque_control(1, torque_1 as f32)?; + motors.send_torque_control(2, torque_2 as f32)?; + + // Increment the command counter + command_count += 2; // Two commands sent per loop iteration + + // 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 f64 / elapsed_time; + println!("Commands per second: {:.2}", commands_per_second); + + // Sleep for a short duration to prevent spamming the loop + std::thread::sleep(Duration::from_millis(100)); + } + + // Reset motors on exit + motors.send_reset(1)?; + motors.send_reset(2)?; - // println!("Program finished"); Ok(()) } From ce50f3ba0cb73882314bb1bf22ad1f54abffc627 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 8 Oct 2024 18:04:01 -0700 Subject: [PATCH 05/10] add sleeping to prevent panic --- actuator/rust/robstride/src/main.rs | 47 +++++++++++++++-------------- 1 file changed, 25 insertions(+), 22 deletions(-) diff --git a/actuator/rust/robstride/src/main.rs b/actuator/rust/robstride/src/main.rs index f4189e2..6f9f971 100644 --- a/actuator/rust/robstride/src/main.rs +++ b/actuator/rust/robstride/src/main.rs @@ -1,7 +1,7 @@ use robstride::{Motor, Motors, RunMode, ROBSTRIDE_CONFIGS}; use std::collections::HashMap; use std::error::Error; -use std::f64::consts::PI; +use std::f32::consts::PI; use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::Arc; use std::time::{Duration, Instant}; @@ -57,34 +57,37 @@ fn main() -> Result<(), Box> { let mut command_count = 0; // Initialize a counter for commands // PD controller parameters - let kp = 3.0; - let kd = 0.0; + 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 = 10.0; - let amplitude = PI / 2.0; + let period = 2.0; + let amplitude = PI; - while running.load(Ordering::SeqCst) && start_time.elapsed() < Duration::new(10, 0) { - let elapsed_time = start_time.elapsed().as_secs_f64(); + while running.load(Ordering::SeqCst) && start_time.elapsed() < Duration::new(20, 0) { + let elapsed_time = start_time.elapsed().as_secs_f32(); // 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 f64; - let current_position_2 = motors.get_latest_feedback(2).map_or(0.0, |f| f.position) as f64; + 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 f64; - let current_velocity_2 = motors.get_latest_feedback(2).map_or(0.0, |f| f.velocity) as f64; + 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 * (desired_position_1 - current_position_1) - kd * current_velocity_1; - let torque_2 = kp * (desired_position_2 - current_position_2) - kd * current_velocity_2; + 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(3)); // Sleep to prevent overwhelming the bus motors.send_torque_control(2, torque_2 as f32)?; // Increment the command counter @@ -93,21 +96,21 @@ fn main() -> Result<(), Box> { // 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(1) { + // println!("Motor 1 Feedback: {:?}", feedback); + // } - if let Some(feedback) = motors.get_latest_feedback(2) { - println!("Motor 2 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 f64 / elapsed_time; + let commands_per_second = command_count as f32 / elapsed_time; println!("Commands per second: {:.2}", commands_per_second); // Sleep for a short duration to prevent spamming the loop - std::thread::sleep(Duration::from_millis(100)); + std::thread::sleep(Duration::from_millis(3)); } // Reset motors on exit From 26808546f3afff07dbb37c02c75ebc93dad2f793 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 8 Oct 2024 18:11:05 -0700 Subject: [PATCH 06/10] current setup --- actuator/rust/robstride/src/main.rs | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/actuator/rust/robstride/src/main.rs b/actuator/rust/robstride/src/main.rs index 6f9f971..14dbea7 100644 --- a/actuator/rust/robstride/src/main.rs +++ b/actuator/rust/robstride/src/main.rs @@ -87,8 +87,9 @@ fn main() -> Result<(), Box> { // Send torque commands to the motors motors.send_torque_control(1, torque_1 as f32)?; - std::thread::sleep(Duration::from_millis(3)); // Sleep to prevent overwhelming the bus + 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 @@ -108,11 +109,13 @@ fn main() -> Result<(), Box> { // Calculate and print the command rate let commands_per_second = command_count as f32 / elapsed_time; println!("Commands per second: {:.2}", commands_per_second); - - // Sleep for a short duration to prevent spamming the loop - std::thread::sleep(Duration::from_millis(3)); } + 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 + // Reset motors on exit motors.send_reset(1)?; motors.send_reset(2)?; From 54abd5503858ea9a2fd244dcdf3f148256dd3ba3 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 8 Oct 2024 22:24:57 -0700 Subject: [PATCH 07/10] draft stand code --- actuator/rust/robstride/src/bin/profile.rs | 197 ++++++++++++--------- actuator/rust/robstride/src/bin/stand.rs | 86 +++++++++ 2 files changed, 203 insertions(+), 80 deletions(-) create mode 100644 actuator/rust/robstride/src/bin/stand.rs diff --git a/actuator/rust/robstride/src/bin/profile.rs b/actuator/rust/robstride/src/bin/profile.rs index 870a8d1..14dbea7 100644 --- a/actuator/rust/robstride/src/bin/profile.rs +++ b/actuator/rust/robstride/src/bin/profile.rs @@ -1,87 +1,124 @@ -use robstride::{Motor, RunMode, ROBSTRIDE_CONFIGS}; -use std::thread; -use std::time::{Duration, Instant}; +use robstride::{Motor, Motors, RunMode, ROBSTRIDE_CONFIGS}; +use std::collections::HashMap; +use std::error::Error; 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 std::sync::Arc; +use std::time::{Duration, Instant}; use ctrlc; -use spin_sleep::SpinSleeper; - -fn calculate_torque(desired_position: f32, actual_position: f32, kp: f32, kd: f32, actual_velocity: f32) -> f32 { - let position_error = desired_position - actual_position; - let velocity_error = -actual_velocity; - kp * position_error + kd * velocity_error -} - -fn main() -> Result<(), Box> { - // println!("Starting sinusoidal profiling"); - - // // Use the updated MotorConfig from ROBSTRIDE_CONFIGS - // let motor_config = ROBSTRIDE_CONFIGS.get("01").expect("Config not found"); - // let mut motor = Motor::new("/dev/ttyCH341USB0", motor_config, 1)?; - - // motor.send_set_mode(RunMode::MitMode)?; - // 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)); - - // motor.send_set_zero()?; - // 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 loop_duration = Duration::from_millis(5); // 200 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"); - - // let sleeper = SpinSleeper::new(1000).with_spin_strategy(spin_sleep::SpinStrategy::YieldThread); - - // 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(); - - // // Read feedback from the motor - // let responses = motor.read_all_pending_responses()?; - // let feedback = responses.get(0).unwrap(); - // println!("{:?}", feedback); - - // let torque = calculate_torque(desired_position, feedback.position, 5.0, 0.1, feedback.velocity); - - // writeln!(file, "{},{},{}", step, desired_position, feedback.position)?; - // // println!("{},{},{},{}", step, desired_position, feedback.position, torque); - - // motor.send_torque_control(torque)?; - - // let elapsed_loop = loop_start.elapsed(); - // if elapsed_loop < loop_duration { - // sleeper.sleep(loop_duration - elapsed_loop); - // } - // step += 1; - // } +fn main() -> Result<(), Box> { + // Create an atomic flag to handle SIGINT + let running = Arc::new(AtomicBool::new(true)); + let r = running.clone(); + + // Set up the SIGINT handler + ctrlc::set_handler(move || { + r.store(false, Ordering::SeqCst); + })?; + + // Create motor instances + let motor1 = Motor::new(&ROBSTRIDE_CONFIGS["04"], 1); + let motor2 = Motor::new(&ROBSTRIDE_CONFIGS["01"], 2); + + // Insert motors into a HashMap + let mut motors_map = HashMap::new(); + motors_map.insert(1, motor1); + motors_map.insert(2, motor2); + + // 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)?; + std::thread::sleep(Duration::from_millis(50)); + motors.send_set_mode(1, RunMode::MitMode)?; + std::thread::sleep(Duration::from_millis(50)); + motors.send_start(1)?; + std::thread::sleep(Duration::from_millis(50)); + motors.send_set_speed_limit(1, 10.0)?; + std::thread::sleep(Duration::from_millis(50)); + motors.send_set_zero(1)?; + + 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; + let amplitude = PI; + + while running.load(Ordering::SeqCst) && start_time.elapsed() < Duration::new(20, 0) { + let elapsed_time = start_time.elapsed().as_secs_f32(); + + // 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 + + // 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); + } + + 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 + + // Reset motors on exit + motors.send_reset(1)?; + motors.send_reset(2)?; - // motor.send_reset()?; - // println!("Sinusoidal profiling finished. Closed cleanly."); Ok(()) } diff --git a/actuator/rust/robstride/src/bin/stand.rs b/actuator/rust/robstride/src/bin/stand.rs new file mode 100644 index 0000000..36a11ea --- /dev/null +++ b/actuator/rust/robstride/src/bin/stand.rs @@ -0,0 +1,86 @@ +use robstride::{Motor, Motors, RunMode, ROBSTRIDE_CONFIGS}; +use std::collections::HashMap; +use std::error::Error; +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::Arc; +use std::time::Duration; +use ctrlc; + +fn main() -> Result<(), Box> { + // Create an atomic flag to handle SIGINT + let running = Arc::new(AtomicBool::new(true)); + let r = running.clone(); + + // Set up the SIGINT handler + ctrlc::set_handler(move || { + r.store(false, Ordering::SeqCst); + })?; + + // Create motor instances for the first Motors group + let mut motors_map_1 = HashMap::new(); + for id in 1..=5 { + let motor = Motor::new(&ROBSTRIDE_CONFIGS["04"], id); + motors_map_1.insert(id, motor); + } + + // Create motor instances for the second Motors group + let mut motors_map_2 = HashMap::new(); + for id in 1..=5 { + let motor = Motor::new(&ROBSTRIDE_CONFIGS["01"], id); + motors_map_2.insert(id, motor); + } + + // 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)?; + + // Function to initialize motors + let initialize_motors = |motors: &mut Motors| -> Result<(), Box> { + 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 + } + motors.read_all_pending_responses()?; + Ok(()) + }; + + let set_location = |motors: &mut Motors, location: f32| -> Result<(), Box> { + for id in 1..=5 { + motors.send_set_location(id, location)?; + std::thread::sleep(Duration::from_millis(50)); + } + motors.read_all_pending_responses()?; + Ok(()) + }; + + // Initialize both groups of motors + initialize_motors(&mut motors_1)?; + initialize_motors(&mut motors_2)?; + + println!("Motors initialized and set to stand position."); + + // 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)?; + } + + // 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)); + } + + println!("Motors reset. Exiting."); + + Ok(()) +} From 81c677683a2fbb5c737532bff5c6da999d9e41ee Mon Sep 17 00:00:00 2001 From: WT-MM Date: Tue, 8 Oct 2024 22:37:27 -0700 Subject: [PATCH 08/10] change command --- actuator/rust/robstride/src/bin/stand.rs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/actuator/rust/robstride/src/bin/stand.rs b/actuator/rust/robstride/src/bin/stand.rs index 36a11ea..4da1448 100644 --- a/actuator/rust/robstride/src/bin/stand.rs +++ b/actuator/rust/robstride/src/bin/stand.rs @@ -32,7 +32,7 @@ fn main() -> Result<(), Box> { // 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)?; + // let mut motors_2 = Motors::new("/dev/ttyCH341USB1", motors_map_2)?; // Function to initialize motors let initialize_motors = |motors: &mut Motors| -> Result<(), Box> { @@ -53,7 +53,7 @@ fn main() -> Result<(), Box> { let set_location = |motors: &mut Motors, location: f32| -> Result<(), Box> { for id in 1..=5 { - motors.send_set_location(id, location)?; + motors.send_position_control(id, location, 5.0)?; std::thread::sleep(Duration::from_millis(50)); } motors.read_all_pending_responses()?; @@ -62,7 +62,7 @@ fn main() -> Result<(), Box> { // Initialize both groups of motors initialize_motors(&mut motors_1)?; - initialize_motors(&mut motors_2)?; + // initialize_motors(&mut motors_2)?; println!("Motors initialized and set to stand position."); @@ -70,13 +70,13 @@ fn main() -> Result<(), Box> { 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)?; + // set_location(&mut motors_2, 0.0)?; } // Reset motors on exit for id in 1..=5 { motors_1.send_reset(id)?; - motors_2.send_reset(id)?; + // motors_2.send_reset(id)?; std::thread::sleep(Duration::from_millis(50)); } From 469e4bfcb90ec6afaacfe6bf7c2775b11b229afe Mon Sep 17 00:00:00 2001 From: WT-MM Date: Wed, 9 Oct 2024 00:07:49 -0700 Subject: [PATCH 09/10] standing robot --- actuator/rust/README.md | 14 ++++ actuator/rust/robstride/src/bin/stand.rs | 81 ++++++++++++++++++------ actuator/rust/robstride/src/lib.rs | 4 +- actuator/rust/robstride/src/main.rs | 35 ++-------- 4 files changed, 81 insertions(+), 53 deletions(-) diff --git a/actuator/rust/README.md b/actuator/rust/README.md index 02c6a27..fa9299c 100644 --- a/actuator/rust/README.md +++ b/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 +``` diff --git a/actuator/rust/robstride/src/bin/stand.rs b/actuator/rust/robstride/src/bin/stand.rs index 4da1448..a440b30 100644 --- a/actuator/rust/robstride/src/bin/stand.rs +++ b/actuator/rust/robstride/src/bin/stand.rs @@ -32,55 +32,96 @@ fn main() -> Result<(), Box> { // 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> { 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> { + let set_location = |motors: &mut Motors, location: f32, kp: f32, kd: f32| -> Result<(), Box> { 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))?; + 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(mut operation: F) -> Result<(), Box> +where + F: FnMut() -> Result<(), Box>, +{ + 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/lib.rs b/actuator/rust/robstride/src/lib.rs index 2487917..9b17c4a 100644 --- a/actuator/rust/robstride/src/lib.rs +++ b/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> { diff --git a/actuator/rust/robstride/src/main.rs b/actuator/rust/robstride/src/main.rs index 14dbea7..db5fb5a 100644 --- a/actuator/rust/robstride/src/main.rs +++ b/actuator/rust/robstride/src/main.rs @@ -18,13 +18,11 @@ fn main() -> Result<(), Box> { })?; // 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> { 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> { // 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> { // 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> { 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(()) } From d05182591185aecb12740f13320f1458adea1187 Mon Sep 17 00:00:00 2001 From: WT-MM Date: Wed, 9 Oct 2024 00:17:27 -0700 Subject: [PATCH 10/10] clean --- actuator/rust/robstride/src/bin/profile.rs | 9 ----- actuator/rust/robstride/src/bin/stand.rs | 40 ++++------------------ actuator/rust/robstride/src/main.rs | 21 ++++++------ 3 files changed, 18 insertions(+), 52 deletions(-) diff --git a/actuator/rust/robstride/src/bin/profile.rs b/actuator/rust/robstride/src/bin/profile.rs index 14dbea7..ecc659a 100644 --- a/actuator/rust/robstride/src/bin/profile.rs +++ b/actuator/rust/robstride/src/bin/profile.rs @@ -97,15 +97,6 @@ fn main() -> Result<(), Box> { // 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 index a440b30..7fcd64f 100644 --- 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,9 +34,8 @@ fn main() -> Result<(), Box> { // 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> { for id in 1..=5 { @@ -42,19 +43,16 @@ fn main() -> Result<(), Box> { 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> { let set_location = |motors: &mut Motors, location: f32, kp: f32, kd: f32| -> Result<(), Box> { for id in 1..=5 { - retry_until_ok(|| motors.send_position_control(id, location, kp, kd).map_err(|e| Box::new(e) as Box))?; - 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> { 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> { Ok(()) } - -fn retry_until_ok(mut operation: F) -> Result<(), Box> -where - F: FnMut() -> Result<(), Box>, -{ - 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 index db5fb5a..1761788 100644 --- 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> { // Create an atomic flag to handle SIGINT let running = Arc::new(AtomicBool::new(true)); @@ -27,15 +28,15 @@ fn main() -> Result<(), Box> { // 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> { 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> { // 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;