diff --git a/Cargo.toml b/Cargo.toml index 010c19a..0e26f27 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -21,6 +21,8 @@ paste = "1.0.10" 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/dxl_sinus.rs b/examples/dxl_sinus.rs new file mode 100644 index 0000000..e64a58a --- /dev/null +++ b/examples/dxl_sinus.rs @@ -0,0 +1,87 @@ +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)?; + mx::write_torque_enable(&io, serial_port.as_mut(), id, 0)?; + + Ok(()) +} 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 {