From 00bb8d59977c2e9cb41a7c06e83bb47d340b58cd Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Thu, 25 May 2023 19:00:06 +0200 Subject: [PATCH 1/4] add tools --- Cargo.toml | 3 +- examples/dxl_sinus.rs | 85 ++++++++++++++++++++++++++ examples/ping.rs | 135 ++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 222 insertions(+), 1 deletion(-) create mode 100644 examples/dxl_sinus.rs create mode 100644 examples/ping.rs diff --git a/Cargo.toml b/Cargo.toml index 41c75cd..29b1d4e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rustypot" -version = "0.3.1" +version = "0.3.0" edition = "2021" license = "Apache-2.0" authors = ["Pollen Robotics"] @@ -19,6 +19,7 @@ log = "0.4.17" paste = "1.0.10" serialport = "4.2.0" clap = { version = "4.0.32", features = ["derive"] } +signal-hook = "0.3.4" [dev-dependencies] env_logger = "0.10.0" diff --git a/examples/dxl_sinus.rs b/examples/dxl_sinus.rs new file mode 100644 index 0000000..4c81689 --- /dev/null +++ b/examples/dxl_sinus.rs @@ -0,0 +1,85 @@ +use std::f32::consts::PI; +use std::time::SystemTime; +use std::{error::Error, thread, time::Duration}; + +use rustypot::device::mx; +use rustypot::DynamixelSerialIO; + +use clap::Parser; +use rustypot::device::mx::conv::radians_to_dxl_pos; + +use signal_hook::flag; + +use std::sync::atomic::{AtomicBool, Ordering}; +use std::sync::Arc; + +#[derive(Parser, Debug)] +#[command(author, version, about, long_about = None)] +struct Args { + /// tty + #[arg(short, long)] + serialport: String, + /// baud + #[arg(short, long, default_value_t = 1_000_000)] + baudrate: u32, + + /// id + #[arg(short, long)] + id: u8, + + ///sinus amplitude (f64) + #[arg(short, long, default_value_t = 10.0)] + amplitude: f32, + + ///sinus frequency (f64) + #[arg(short, long, default_value_t = 1.0)] + frequency: f32, +} + +fn main() -> Result<(), Box> { + let args = Args::parse(); + let serialportname: String = args.serialport; + let baudrate: u32 = args.baudrate; + let id: u8 = args.id; + let amplitude: f32 = args.amplitude; + let frequency: f32 = args.frequency; + + //print all the argument values + println!("serialport: {}", serialportname); + println!("baudrate: {}", baudrate); + println!("id: {}", id); + println!("amplitude: {}", amplitude); + println!("frequency: {}", frequency); + let term = Arc::new(AtomicBool::new(false)); + + flag::register(signal_hook::consts::SIGINT, Arc::clone(&term))?; + + let mut serial_port = serialport::new(serialportname, baudrate) + .timeout(Duration::from_millis(10)) + .open()?; + println!("serial port opened"); + + let io = DynamixelSerialIO::v1(); + + let x: i16 = mx::read_present_position(&io, serial_port.as_mut(), id)?; + println!("present pos: {}", x); + + mx::write_torque_enable(&io, serial_port.as_mut(), id, 1)?; + + let now = SystemTime::now(); + while !term.load(Ordering::Relaxed) { + let t = now.elapsed().unwrap().as_secs_f32(); + let target = amplitude * (2.0 * PI * frequency * t).sin().to_radians(); + println!("target: {}", target); + mx::write_goal_position( + &io, + serial_port.as_mut(), + id, + radians_to_dxl_pos(target.into()), + )?; + + thread::sleep(Duration::from_millis(10)); + } + // mx::write_torque_enable(&io, serial_port.as_mut(), id, false)?; + Ok(()) +} diff --git a/examples/ping.rs b/examples/ping.rs new file mode 100644 index 0000000..61885b2 --- /dev/null +++ b/examples/ping.rs @@ -0,0 +1,135 @@ +use std::f32::consts::PI; +use std::time::SystemTime; +use std::{error::Error, thread, time::Duration}; + +use clap::Parser; +use rustypot::device::orbita_foc::{self, DiskValue}; +use rustypot::DynamixelSerialIO; + +#[derive(Parser, Debug)] +#[command(author, version, about, long_about = None)] +struct Args { + /// tty + #[arg(short, long)] + serialport: String, + /// baud + #[arg(short, long, default_value_t = 1_000_000)] + baudrate: u32, +} + +fn main() -> Result<(), Box> { + env_logger::init(); + let args = Args::parse(); + let serialportname: String = args.serialport; + let baudrate: u32 = args.baudrate; + //print all the argument values + println!("serialport: {}", serialportname); + println!("baudrate: {}", baudrate); + let mut serial_port = serialport::new(serialportname, baudrate) + .timeout(Duration::from_millis(20)) + .open()?; + + let io = DynamixelSerialIO::v1(); + + for id in 1..254 { + let x = io.ping(serial_port.as_mut(), id); + println!("{id} {:?}", x); + thread::sleep(Duration::from_millis(5)); + // match x { + // Ok(v) => { + // if v == true { + // break; + // } + // } + // Err(_) => {} + // } + } + Ok(()) + + // let id = 70; + + // let now = SystemTime::now(); + // let x = io.ping(serial_port.as_mut(), id); + // println!("{:?}", x); + + // // orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 1)?; + // let mot_driv_state = orbita_foc::read_motors_drivers_states(&io, serial_port.as_mut(), id)?; + // println!("motors/drivers states: {:#010b}", mot_driv_state); // 10 chars for u8 since it integers "0x" + // let init_pos = orbita_foc::read_present_position(&io, serial_port.as_mut(), id)?; + + // println!("init_pos: {:?}", init_pos); + // thread::sleep(Duration::from_millis(3000)); + // // let reset = 0; + // loop { + // // let x = io.ping(serial_port.as_mut(), id); + // // println!("{:?}", x); + + // // let pos = orbita_foc::read_present_position(&io, serial_port.as_mut(), id)?; + + // let t = now.elapsed().unwrap().as_secs_f32(); + // let target = 1.0_f32 * (2.0 * PI * 0.12 * t).sin(); // large slow complete sinus + // // let target = 4.267 * 180.0_f32.to_radians() * (2.0 * PI * 0.1 * t).sin(); // small fast complete sinus + // // // let target = 1.0_f32 * (2.0 * PI * 10.0 * t).sin(); // incredible shaky Orbita + // // println!( + // // "{:?} {:?} | disks {:?} {:?} {:?}", + // // t, + // // target, + // // pos.top.to_degrees() / (64.0 / 15.0), + // // pos.middle.to_degrees() / (64.0 / 15.0), + // // pos.bottom.to_degrees() / (64.0 / 15.0) + // // ); + // // orbita_foc::write_top_goal_position(&io, serial_port.as_mut(), id, target)?; + // // println!("{}", t); + + // // orbita_foc::write_goal_position( + // // &io, + // // serial_port.as_mut(), + // // id, + // // DiskValue { + // // top: init_pos.top + target, + // // middle: init_pos.middle + target, + // // bottom: init_pos.bottom + target, + // // }, + // // )?; + + // let pos = orbita_foc::read_present_position(&io, serial_port.as_mut(), id)?; + // println!( + // "top {:.3} mid {:.3} bot {:.3}", + // pos.top, pos.middle, pos.bottom + // ); + // // let state = orbita_foc::read_motors_drivers_states(&io, serial_port.as_mut(), id)?; + // // println!("state: {:?}", state); + + // // thread::sleep(Duration::from_millis(10)); + // // thread::sleep(Duration::from_micros(10)); + + // // let target = 4.267 * 90.0_f32.to_radians(); + + // // if t < 5.0 { + // // orbita_foc::write_goal_position( + // // &io, + // // serial_port.as_mut(), + // // id, + // // DiskValue { + // // top: init_pos.top + target, + // // middle: init_pos.middle + target, + // // bottom: init_pos.bottom + target, + // // }, + // // )?; + // // } + // // // thread::sleep(Duration::from_millis(3000)); + // // else { + // // orbita_foc::write_goal_position( + // // &io, + // // serial_port.as_mut(), + // // id, + // // DiskValue { + // // top: init_pos.top, + // // middle: init_pos.middle, + // // bottom: init_pos.bottom, + // // }, + // // )?; + // // } + // // thread::sleep(Duration::from_millis(3000)); + // } +} From 1959f5bccb8b4bd87e170d74132f2992ff82e8b0 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Thu, 25 May 2023 19:04:35 +0200 Subject: [PATCH 2/4] torque off after ctrl-c in sinus --- examples/dxl_sinus.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/dxl_sinus.rs b/examples/dxl_sinus.rs index 4c81689..e64a58a 100644 --- a/examples/dxl_sinus.rs +++ b/examples/dxl_sinus.rs @@ -81,5 +81,7 @@ fn main() -> Result<(), Box> { thread::sleep(Duration::from_millis(10)); } // mx::write_torque_enable(&io, serial_port.as_mut(), id, false)?; + mx::write_torque_enable(&io, serial_port.as_mut(), id, 0)?; + Ok(()) } From d777d864770fcd5463ca9fe8c5695b9f33cef9b6 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Mon, 19 Jun 2023 15:20:10 +0200 Subject: [PATCH 3/4] improve print --- examples/ping.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ping.rs b/examples/ping.rs index 61885b2..a04027d 100644 --- a/examples/ping.rs +++ b/examples/ping.rs @@ -33,7 +33,7 @@ fn main() -> Result<(), Box> { for id in 1..254 { let x = io.ping(serial_port.as_mut(), id); - println!("{id} {:?}", x); + println!("{id:>4} {:?}", x); thread::sleep(Duration::from_millis(5)); // match x { // Ok(v) => { From c2199adc8848a10573620f99bff69041245c136c Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Thu, 26 Sep 2024 14:14:00 +0200 Subject: [PATCH 4/4] clean scan --- Cargo.toml | 2 +- examples/ping.rs | 135 -------------------------------------------- src/bin/dxl_scan.rs | 56 +++++++++++++++--- src/device/mod.rs | 49 ++++++++++++++++ 4 files changed, 97 insertions(+), 145 deletions(-) delete mode 100644 examples/ping.rs diff --git a/Cargo.toml b/Cargo.toml index 521d227..0e26f27 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -22,7 +22,7 @@ serialport = "4.2.0" clap = { version = "4.0.32", features = ["derive"] } proc-macro2 = { version = "=1.0.67", features=["default", "proc-macro"] } signal-hook = "0.3.4" - +num_enum = "0.7.3" [dev-dependencies] env_logger = "0.10.0" diff --git a/examples/ping.rs b/examples/ping.rs deleted file mode 100644 index a04027d..0000000 --- a/examples/ping.rs +++ /dev/null @@ -1,135 +0,0 @@ -use std::f32::consts::PI; -use std::time::SystemTime; -use std::{error::Error, thread, time::Duration}; - -use clap::Parser; -use rustypot::device::orbita_foc::{self, DiskValue}; -use rustypot::DynamixelSerialIO; - -#[derive(Parser, Debug)] -#[command(author, version, about, long_about = None)] -struct Args { - /// tty - #[arg(short, long)] - serialport: String, - /// baud - #[arg(short, long, default_value_t = 1_000_000)] - baudrate: u32, -} - -fn main() -> Result<(), Box> { - env_logger::init(); - let args = Args::parse(); - let serialportname: String = args.serialport; - let baudrate: u32 = args.baudrate; - //print all the argument values - println!("serialport: {}", serialportname); - println!("baudrate: {}", baudrate); - let mut serial_port = serialport::new(serialportname, baudrate) - .timeout(Duration::from_millis(20)) - .open()?; - - let io = DynamixelSerialIO::v1(); - - for id in 1..254 { - let x = io.ping(serial_port.as_mut(), id); - println!("{id:>4} {:?}", x); - thread::sleep(Duration::from_millis(5)); - // match x { - // Ok(v) => { - // if v == true { - // break; - // } - // } - // Err(_) => {} - // } - } - Ok(()) - - // let id = 70; - - // let now = SystemTime::now(); - // let x = io.ping(serial_port.as_mut(), id); - // println!("{:?}", x); - - // // orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 1)?; - // let mot_driv_state = orbita_foc::read_motors_drivers_states(&io, serial_port.as_mut(), id)?; - // println!("motors/drivers states: {:#010b}", mot_driv_state); // 10 chars for u8 since it integers "0x" - // let init_pos = orbita_foc::read_present_position(&io, serial_port.as_mut(), id)?; - - // println!("init_pos: {:?}", init_pos); - // thread::sleep(Duration::from_millis(3000)); - // // let reset = 0; - // loop { - // // let x = io.ping(serial_port.as_mut(), id); - // // println!("{:?}", x); - - // // let pos = orbita_foc::read_present_position(&io, serial_port.as_mut(), id)?; - - // let t = now.elapsed().unwrap().as_secs_f32(); - // let target = 1.0_f32 * (2.0 * PI * 0.12 * t).sin(); // large slow complete sinus - // // let target = 4.267 * 180.0_f32.to_radians() * (2.0 * PI * 0.1 * t).sin(); // small fast complete sinus - // // // let target = 1.0_f32 * (2.0 * PI * 10.0 * t).sin(); // incredible shaky Orbita - // // println!( - // // "{:?} {:?} | disks {:?} {:?} {:?}", - // // t, - // // target, - // // pos.top.to_degrees() / (64.0 / 15.0), - // // pos.middle.to_degrees() / (64.0 / 15.0), - // // pos.bottom.to_degrees() / (64.0 / 15.0) - // // ); - // // orbita_foc::write_top_goal_position(&io, serial_port.as_mut(), id, target)?; - // // println!("{}", t); - - // // orbita_foc::write_goal_position( - // // &io, - // // serial_port.as_mut(), - // // id, - // // DiskValue { - // // top: init_pos.top + target, - // // middle: init_pos.middle + target, - // // bottom: init_pos.bottom + target, - // // }, - // // )?; - - // let pos = orbita_foc::read_present_position(&io, serial_port.as_mut(), id)?; - // println!( - // "top {:.3} mid {:.3} bot {:.3}", - // pos.top, pos.middle, pos.bottom - // ); - // // let state = orbita_foc::read_motors_drivers_states(&io, serial_port.as_mut(), id)?; - // // println!("state: {:?}", state); - - // // thread::sleep(Duration::from_millis(10)); - // // thread::sleep(Duration::from_micros(10)); - - // // let target = 4.267 * 90.0_f32.to_radians(); - - // // if t < 5.0 { - // // orbita_foc::write_goal_position( - // // &io, - // // serial_port.as_mut(), - // // id, - // // DiskValue { - // // top: init_pos.top + target, - // // middle: init_pos.middle + target, - // // bottom: init_pos.bottom + target, - // // }, - // // )?; - // // } - // // // thread::sleep(Duration::from_millis(3000)); - // // else { - // // orbita_foc::write_goal_position( - // // &io, - // // serial_port.as_mut(), - // // id, - // // DiskValue { - // // top: init_pos.top, - // // middle: init_pos.middle, - // // bottom: init_pos.bottom, - // // }, - // // )?; - // // } - // // thread::sleep(Duration::from_millis(3000)); - // } -} diff --git a/src/bin/dxl_scan.rs b/src/bin/dxl_scan.rs index 23bc5eb..ecd520f 100644 --- a/src/bin/dxl_scan.rs +++ b/src/bin/dxl_scan.rs @@ -1,12 +1,19 @@ use clap::{Parser, ValueEnum}; +use std::collections::HashMap; use std::{error::Error, time::Duration}; +use rustypot::device::DxlModel; use rustypot::DynamixelSerialIO; - #[derive(Parser, Debug)] +#[command(author, version, about, long_about = None)] struct Args { - serial_port: String, - #[arg(value_enum)] + #[arg(short, long, default_value = "/dev/ttyUSB0")] + serialport: String, + /// baud + #[arg(short, long, default_value_t = 2_000_000)] + baudrate: u32, + + #[arg(short, long, value_enum, default_value_t = ProtocolVersion::V1)] protocol: ProtocolVersion, } @@ -18,21 +25,52 @@ enum ProtocolVersion { fn main() -> Result<(), Box> { let args = Args::parse(); + let serialport: String = args.serialport; + let baudrate: u32 = args.baudrate; + let protocol: ProtocolVersion = args.protocol; + + //print the standard ids for the arm motors + //print all the argument values + println!("serialport: {}", serialport); + println!("baudrate: {}", baudrate); + match protocol { + ProtocolVersion::V1 => println!("protocol: V1"), + ProtocolVersion::V2 => println!("protocol: V2"), + } + + let mut found = HashMap::new(); println!("Scanning..."); - let mut serial_port = serialport::new(args.serial_port, 2_000_000) + let mut serial_port = serialport::new(serialport, baudrate) .timeout(Duration::from_millis(10)) .open()?; - let io = match args.protocol { + let io = match protocol { ProtocolVersion::V1 => DynamixelSerialIO::v1(), ProtocolVersion::V2 => DynamixelSerialIO::v2(), }; - let ids: Vec = (1..253) - .filter(|id| io.ping(serial_port.as_mut(), *id).unwrap()) - .collect(); - println!("Ids found: {:?}", ids); + for id in 1..253 { + match io.ping(serial_port.as_mut(), id) { + Ok(present) => { + if present { + let model = io.read(serial_port.as_mut(), id, 0, 2).unwrap(); + + found.insert(id, u16::from_le_bytes([model[0], model[1]])); + } + } + Err(e) => eprintln!("Error: {e}"), + }; + } + + println!("found {} motors", found.len()); + for (key, value) in found { + println!( + "id: {} model: {:?}", + key, + DxlModel::try_from(value).unwrap() + ); + } Ok(()) } diff --git a/src/device/mod.rs b/src/device/mod.rs index 191cfb1..7feae52 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -1,10 +1,59 @@ //! High-level register access functions for a specific dynamixel device +use num_enum::IntoPrimitive; +use num_enum::TryFromPrimitive; + use paste::paste; use std::mem::size_of; use crate::{reg_read_only, reg_read_write, DynamixelSerialIO, Result}; +#[derive(Debug, IntoPrimitive, TryFromPrimitive)] +#[repr(u16)] +pub enum DxlModel { + AX12A = 12, + AX12W = 300, + AX18A = 18, + RX10 = 10, + RX24F = 24, + RX28 = 28, + RX64 = 64, + EX106 = 107, + MX12W = 360, + MX28 = 29, + MX282 = 30, + MX64 = 310, + MX642 = 311, + MX106 = 320, + MX1062 = 321, + XL320 = 350, + XL330M077 = 1190, + XL330M288 = 1200, + XC330M181 = 1230, + XC330M288 = 1240, + XC330T181 = 1210, + XC330T288 = 1220, + XL430W250 = 1060, + XL430W2502 = 1090, + XC430W2502 = 1160, + XC430W150 = 1070, + XC430W240 = 1080, + XM430W210 = 1030, + XM430W350 = 1020, + XM540W150 = 1130, + XM540W270 = 1120, + XH430W210 = 1010, + XH430W350 = 1000, + XH430V210 = 1050, + XH430V350 = 1040, + XH540W150 = 1110, + XH540W270 = 1100, + XH540V150 = 1150, + XH540V270 = 1140, + XW540T260 = 1170, + XW540T140 = 1180, +} + /// Generates read and sync_read functions for given register #[macro_export] macro_rules! reg_read_only {