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 {