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

Support poulpe

Author: SteveNguyen

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "rustypot"
-version = "0.4.2"
+version = "0.5.0"
 edition = "2021"
 license = "Apache-2.0"
 authors = ["Pollen Robotics"]
@@ -10,6 +10,7 @@ repository = "https://github.com/pollen-robotics/rustypot"
 readme = "README.md"
 keywords = ["robotics", "dynamixel"]
 categories = ["science::robotics"]
+#autoexamples = false #disable auto discovery of examples
 
 
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
@@ -19,6 +20,7 @@ log = "0.4.17"
 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"] }
 
 [dev-dependencies]
 env_logger = "0.10.0"

Changelog.md

@@ -1,3 +1,8 @@
+## Version 0.5.0
+
+- Add an post delay option to the read and write method.
+- Add dxl XM motor device
+
 ## Version 0.4.0
 
 - Add support for orbita-2dof-foc device.
@@ -18,4 +23,4 @@
 
 - Support protocol v1 and v2
 - Support read, sync read, write, sync write, ping
-- Support mx and xl-320
+- Support mx and xl-320

examples/foc.rs

@@ -10,39 +10,67 @@ fn main() -> Result<(), Box<dyn Error>> {
         .timeout(Duration::from_millis(100))
         .open()?;
 
-    let io = DynamixelSerialIO::v1();
+    let io = DynamixelSerialIO::v1().with_post_delay(Duration::from_millis(1));
 
     let id = 70;
-    orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 0)?;
+    let mut state = orbita_foc::read_motors_drivers_states(&io, serial_port.as_mut(), id)?;
+    println!("state {:#010b}", state);
+    println!("state {:?}", state);
+
+    let fw = orbita_foc::read_firmware_version(&io, serial_port.as_mut(), id)?;
+    println!("Firmware version {:?}", fw);
+
+    orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 1)?;
+    thread::sleep(Duration::from_millis(1000));
+    // orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 0)?;
 
-    thread::sleep(Duration::from_millis(100));
+    // thread::sleep(Duration::from_millis(1000));
+    // orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 1)?;
 
+    // thread::sleep(Duration::from_millis(1000));
+
+    state = orbita_foc::read_motors_drivers_states(&io, serial_port.as_mut(), id)?;
+    println!("state {:#010b}", state);
     let now = SystemTime::now();
     // let x = io.ping(serial_port.as_mut(), id);
     // println!("{:?}", x);
-    loop {
+    let mut total = 0.0;
+
+    while total < 10.0 {
         // let x = io.ping(serial_port.as_mut(), id);
         // println!("{:?}", x);
 
         let pos = orbita_foc::read_present_position(&io, serial_port.as_mut(), id)?;
-        println!("{:?}", pos);
+
+        let tophall = orbita_foc::read_top_present_hall(&io, serial_port.as_mut(), id)?;
+        let midhall = orbita_foc::read_middle_present_hall(&io, serial_port.as_mut(), id)?;
+        let bothall = orbita_foc::read_bottom_present_hall(&io, serial_port.as_mut(), id)?;
+        println!(
+            "{:?} tophall: {:?} midhal: {:?} bothall: {:?}",
+            pos, tophall, midhall, bothall
+        );
 
         let t = now.elapsed().unwrap().as_secs_f32();
-        let target = 60.0_f32 * (2.0 * PI * 0.5 * t).sin();
-        orbita_foc::write_top_goal_position(&io, serial_port.as_mut(), id, target)?;
-        // println!("{}", t);
+        let target = 10.0_f32 * (2.0 * PI * 0.25 * t).sin();
 
         orbita_foc::write_goal_position(
             &io,
             serial_port.as_mut(),
             id,
             DiskValue {
-                top: target,
-                middle: target,
-                bottom: target,
+                top: target.to_radians() + pos.top,
+                middle: target.to_radians() + pos.middle,
+                bottom: target.to_radians() + pos.bottom,
+                // top: pos.top,
+                // middle: pos.middle,
+                // bottom: pos.bottom,
             },
         )?;
-
         thread::sleep(Duration::from_millis(10));
+        total += 0.01;
     }
+    orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 0)?;
+
+    thread::sleep(Duration::from_millis(1000));
+    Ok(())
 }

examples/poulpe2d.rs

@@ -0,0 +1,166 @@
+use std::time::SystemTime;
+use std::{error::Error, thread, time::Duration, time::Instant};
+
+use rustypot::device::orbita2d_poulpe::{self, MotorValue};
+// use rustypot::device::orbita3d_poulpe::{self, MotorValue};
+use rustypot::DynamixelSerialIO;
+
+use clap::Parser;
+
+#[derive(Parser, Debug)]
+#[command(author, version, about, long_about = None)]
+struct Args {
+    /// tty
+    #[arg(short, long, default_value = "/dev/ttyUSB0")]
+    serialport: String,
+    /// baud
+    #[arg(short, long, default_value_t = 2_000_000)]
+    baudrate: u32,
+
+    /// id
+    #[arg(short, long, default_value_t = 43)]
+    id: u8,
+
+    ///sinus amplitude (f64)
+    #[arg(short, long, default_value_t = 1.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 the standard ids for the arm motors
+
+    //print all the argument values
+    println!("serialport: {}", serialportname);
+    println!("baudrate: {}", baudrate);
+    println!("id: {}", id);
+    println!("amplitude: {}", amplitude);
+    println!("frequency: {}", frequency);
+
+    let mut serial_port = serialport::new(serialportname, baudrate)
+        .timeout(Duration::from_millis(10))
+        .open()?;
+
+    let now = SystemTime::now();
+
+    let io = DynamixelSerialIO::v1();
+
+    // Ping
+    let x = io.ping(serial_port.as_mut(), id);
+    println!("Ping {:?}: {:?}", id, x);
+    thread::sleep(Duration::from_millis(100));
+
+    let _ = orbita2d_poulpe::write_torque_enable(
+        &io,
+        serial_port.as_mut(),
+        id,
+        MotorValue::<bool> {
+            motor_a: true,
+            motor_b: true,
+        },
+    )?;
+    thread::sleep(Duration::from_millis(1000));
+    // let torque = orbita3d_poulpe::read_torque_enable(&io, serial_port.as_mut(), id)?;
+    // println!("torque: {:?}", torque);
+    // thread::sleep(Duration::from_millis(1000));
+
+    let curr_pos = orbita2d_poulpe::read_current_position(&io, serial_port.as_mut(), id)?;
+
+    println!("curr_pos: {:?} {:?}", curr_pos.motor_a, curr_pos.motor_b);
+
+    // let index_sensor = orbita3d_poulpe::read_index_sensor(&io, serial_port.as_mut(), id)?;
+    // println!("index_sensor: {:?} {:?} {:?}", index_sensor.top, index_sensor.middle, index_sensor.bottom);
+
+    let mut t = now.elapsed().unwrap().as_secs_f32();
+    let mut nberr = 0;
+    let mut nbtot = 0;
+    let mut nbok = 0;
+    let mut target = 0.0;
+    loop {
+        let t0 = Instant::now();
+
+        if t > 3.0 {
+            break;
+        }
+
+        t = now.elapsed().unwrap().as_secs_f32();
+        // let target = amplitude * 180.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin();
+        target += 0.001;
+        // let feedback = orbita2d_poulpe::write_target_position(&io, serial_port.as_mut(), id, MotorValue::<f32>{motor_a:target+curr_pos.motor_a, motor_b:target+curr_pos.motor_b});
+
+        // let feedback = orbita2d_poulpe::write_target_position(
+        //     &io,
+        //     serial_port.as_mut(),
+        //     id,
+        //     MotorValue::<f32> {
+        //         motor_a: target + curr_pos.motor_a,
+        //         motor_b: curr_pos.motor_b,
+        //     },
+        // );
+
+        let feedback = orbita2d_poulpe::write_target_position(
+            &io,
+            serial_port.as_mut(),
+            id,
+            MotorValue::<f32> {
+                motor_a: target + curr_pos.motor_a,
+                motor_b: target + curr_pos.motor_b,
+            },
+        );
+
+        match feedback {
+            Ok(feedback) => {
+                nbok += 1;
+                println!(
+                    "42 target: {} feedback pos: {} {}",
+                    target, feedback.position.motor_a, feedback.position.motor_b,
+                );
+            }
+            Err(e) => {
+                nberr += 1;
+                println!("error: {:?}", e);
+            }
+        }
+
+        nbtot += 1;
+
+        // thread::sleep(Duration::from_micros(1000-t0.elapsed().as_micros() as u64));
+        // thread::sleep(Duration::from_millis(1));
+        thread::sleep(Duration::from_micros(500));
+        // thread::sleep(Duration::from_micros(4500));
+        println!("ELAPSED: {:?}", t0.elapsed().as_micros());
+    }
+
+    println!(
+        "nberr: {} nbtot: {} nbok: {} ratio: {:?}",
+        nberr,
+        nbtot,
+        nbok,
+        nbok as f32 / nbtot as f32
+    );
+
+    println!("STOP");
+    let _ = orbita2d_poulpe::write_torque_enable(
+        &io,
+        serial_port.as_mut(),
+        id,
+        MotorValue::<bool> {
+            motor_a: false,
+            motor_b: false,
+        },
+    )?;
+
+    thread::sleep(Duration::from_millis(2000));
+
+    Ok(())
+}