clean scan

Author: Steve Nguyen

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"

examples/ping.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<dyn Error>> {
     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<u8> = (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(())
 }

src/bin/dxl_scan.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 {