Merge pull request #61 from pollen-robotics/support-poulpe

Small modif to scan tool

Author: RemiFabre

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"

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<dyn Error>> {
+    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(())
+}

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<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/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 {