Skip to content

Commit d3c32b0

Browse files
author
Steve Nguyen
committed
Merge branch '30-ping-and-sin-generic-tools' into support-poulpe
2 parents 781de4e + d777d86 commit d3c32b0

File tree

3 files changed

+224
-0
lines changed

3 files changed

+224
-0
lines changed

‎Cargo.toml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,8 @@ paste = "1.0.10"
2121
serialport = "4.2.0"
2222
clap = { version = "4.0.32", features = ["derive"] }
2323
proc-macro2 = { version = "=1.0.67", features=["default", "proc-macro"] }
24+
signal-hook = "0.3.4"
25+
2426

2527
[dev-dependencies]
2628
env_logger = "0.10.0"

‎examples/dxl_sinus.rs‎

Lines changed: 87 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,87 @@
1+
use std::f32::consts::PI;
2+
use std::time::SystemTime;
3+
use std::{error::Error, thread, time::Duration};
4+
5+
use rustypot::device::mx;
6+
use rustypot::DynamixelSerialIO;
7+
8+
use clap::Parser;
9+
use rustypot::device::mx::conv::radians_to_dxl_pos;
10+
11+
use signal_hook::flag;
12+
13+
use std::sync::atomic::{AtomicBool, Ordering};
14+
use std::sync::Arc;
15+
16+
#[derive(Parser, Debug)]
17+
#[command(author, version, about, long_about = None)]
18+
struct Args {
19+
/// tty
20+
#[arg(short, long)]
21+
serialport: String,
22+
/// baud
23+
#[arg(short, long, default_value_t = 1_000_000)]
24+
baudrate: u32,
25+
26+
/// id
27+
#[arg(short, long)]
28+
id: u8,
29+
30+
///sinus amplitude (f64)
31+
#[arg(short, long, default_value_t = 10.0)]
32+
amplitude: f32,
33+
34+
///sinus frequency (f64)
35+
#[arg(short, long, default_value_t = 1.0)]
36+
frequency: f32,
37+
}
38+
39+
fn main() -> Result<(), Box<dyn Error>> {
40+
let args = Args::parse();
41+
let serialportname: String = args.serialport;
42+
let baudrate: u32 = args.baudrate;
43+
let id: u8 = args.id;
44+
let amplitude: f32 = args.amplitude;
45+
let frequency: f32 = args.frequency;
46+
47+
//print all the argument values
48+
println!("serialport: {}", serialportname);
49+
println!("baudrate: {}", baudrate);
50+
println!("id: {}", id);
51+
println!("amplitude: {}", amplitude);
52+
println!("frequency: {}", frequency);
53+
let term = Arc::new(AtomicBool::new(false));
54+
55+
flag::register(signal_hook::consts::SIGINT, Arc::clone(&term))?;
56+
57+
let mut serial_port = serialport::new(serialportname, baudrate)
58+
.timeout(Duration::from_millis(10))
59+
.open()?;
60+
println!("serial port opened");
61+
62+
let io = DynamixelSerialIO::v1();
63+
64+
let x: i16 = mx::read_present_position(&io, serial_port.as_mut(), id)?;
65+
println!("present pos: {}", x);
66+
67+
mx::write_torque_enable(&io, serial_port.as_mut(), id, 1)?;
68+
69+
let now = SystemTime::now();
70+
while !term.load(Ordering::Relaxed) {
71+
let t = now.elapsed().unwrap().as_secs_f32();
72+
let target = amplitude * (2.0 * PI * frequency * t).sin().to_radians();
73+
println!("target: {}", target);
74+
mx::write_goal_position(
75+
&io,
76+
serial_port.as_mut(),
77+
id,
78+
radians_to_dxl_pos(target.into()),
79+
)?;
80+
81+
thread::sleep(Duration::from_millis(10));
82+
}
83+
// mx::write_torque_enable(&io, serial_port.as_mut(), id, false)?;
84+
mx::write_torque_enable(&io, serial_port.as_mut(), id, 0)?;
85+
86+
Ok(())
87+
}

‎examples/ping.rs‎

Lines changed: 135 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,135 @@
1+
use std::f32::consts::PI;
2+
use std::time::SystemTime;
3+
use std::{error::Error, thread, time::Duration};
4+
5+
use clap::Parser;
6+
use rustypot::device::orbita_foc::{self, DiskValue};
7+
use rustypot::DynamixelSerialIO;
8+
9+
#[derive(Parser, Debug)]
10+
#[command(author, version, about, long_about = None)]
11+
struct Args {
12+
/// tty
13+
#[arg(short, long)]
14+
serialport: String,
15+
/// baud
16+
#[arg(short, long, default_value_t = 1_000_000)]
17+
baudrate: u32,
18+
}
19+
20+
fn main() -> Result<(), Box<dyn Error>> {
21+
env_logger::init();
22+
let args = Args::parse();
23+
let serialportname: String = args.serialport;
24+
let baudrate: u32 = args.baudrate;
25+
//print all the argument values
26+
println!("serialport: {}", serialportname);
27+
println!("baudrate: {}", baudrate);
28+
let mut serial_port = serialport::new(serialportname, baudrate)
29+
.timeout(Duration::from_millis(20))
30+
.open()?;
31+
32+
let io = DynamixelSerialIO::v1();
33+
34+
for id in 1..254 {
35+
let x = io.ping(serial_port.as_mut(), id);
36+
println!("{id:>4} {:?}", x);
37+
thread::sleep(Duration::from_millis(5));
38+
// match x {
39+
// Ok(v) => {
40+
// if v == true {
41+
// break;
42+
// }
43+
// }
44+
// Err(_) => {}
45+
// }
46+
}
47+
Ok(())
48+
49+
// let id = 70;
50+
51+
// let now = SystemTime::now();
52+
// let x = io.ping(serial_port.as_mut(), id);
53+
// println!("{:?}", x);
54+
55+
// // orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 1)?;
56+
// let mot_driv_state = orbita_foc::read_motors_drivers_states(&io, serial_port.as_mut(), id)?;
57+
// println!("motors/drivers states: {:#010b}", mot_driv_state); // 10 chars for u8 since it integers "0x"
58+
// let init_pos = orbita_foc::read_present_position(&io, serial_port.as_mut(), id)?;
59+
60+
// println!("init_pos: {:?}", init_pos);
61+
// thread::sleep(Duration::from_millis(3000));
62+
// // let reset = 0;
63+
// loop {
64+
// // let x = io.ping(serial_port.as_mut(), id);
65+
// // println!("{:?}", x);
66+
67+
// // let pos = orbita_foc::read_present_position(&io, serial_port.as_mut(), id)?;
68+
69+
// let t = now.elapsed().unwrap().as_secs_f32();
70+
// let target = 1.0_f32 * (2.0 * PI * 0.12 * t).sin(); // large slow complete sinus
71+
// // let target = 4.267 * 180.0_f32.to_radians() * (2.0 * PI * 0.1 * t).sin(); // small fast complete sinus
72+
// // // let target = 1.0_f32 * (2.0 * PI * 10.0 * t).sin(); // incredible shaky Orbita
73+
// // println!(
74+
// // "{:?} {:?} | disks {:?} {:?} {:?}",
75+
// // t,
76+
// // target,
77+
// // pos.top.to_degrees() / (64.0 / 15.0),
78+
// // pos.middle.to_degrees() / (64.0 / 15.0),
79+
// // pos.bottom.to_degrees() / (64.0 / 15.0)
80+
// // );
81+
// // orbita_foc::write_top_goal_position(&io, serial_port.as_mut(), id, target)?;
82+
// // println!("{}", t);
83+
84+
// // orbita_foc::write_goal_position(
85+
// // &io,
86+
// // serial_port.as_mut(),
87+
// // id,
88+
// // DiskValue {
89+
// // top: init_pos.top + target,
90+
// // middle: init_pos.middle + target,
91+
// // bottom: init_pos.bottom + target,
92+
// // },
93+
// // )?;
94+
95+
// let pos = orbita_foc::read_present_position(&io, serial_port.as_mut(), id)?;
96+
// println!(
97+
// "top {:.3} mid {:.3} bot {:.3}",
98+
// pos.top, pos.middle, pos.bottom
99+
// );
100+
// // let state = orbita_foc::read_motors_drivers_states(&io, serial_port.as_mut(), id)?;
101+
// // println!("state: {:?}", state);
102+
103+
// // thread::sleep(Duration::from_millis(10));
104+
// // thread::sleep(Duration::from_micros(10));
105+
106+
// // let target = 4.267 * 90.0_f32.to_radians();
107+
108+
// // if t < 5.0 {
109+
// // orbita_foc::write_goal_position(
110+
// // &io,
111+
// // serial_port.as_mut(),
112+
// // id,
113+
// // DiskValue {
114+
// // top: init_pos.top + target,
115+
// // middle: init_pos.middle + target,
116+
// // bottom: init_pos.bottom + target,
117+
// // },
118+
// // )?;
119+
// // }
120+
// // // thread::sleep(Duration::from_millis(3000));
121+
// // else {
122+
// // orbita_foc::write_goal_position(
123+
// // &io,
124+
// // serial_port.as_mut(),
125+
// // id,
126+
// // DiskValue {
127+
// // top: init_pos.top,
128+
// // middle: init_pos.middle,
129+
// // bottom: init_pos.bottom,
130+
// // },
131+
// // )?;
132+
// // }
133+
// // thread::sleep(Duration::from_millis(3000));
134+
// }
135+
}

0 commit comments

Comments
 (0)