test poulpe

Cargo.toml

@@ -19,6 +19,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"

examples/foc.rs

@@ -13,36 +13,64 @@ fn main() -> Result<(), Box<dyn Error>> {
     let io = DynamixelSerialIO::v1();
 
     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/poulpe.rs

@@ -0,0 +1,96 @@
+use std::f32::consts::PI;
+use std::time::SystemTime;
+use std::{error::Error, thread, time::Duration};
+
+use rustypot::device::poulpe::{self};
+use rustypot::DynamixelSerialIO;
+
+use clap::Parser;
+
+#[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,
+}
+
+const MOTOR_A: u8 = 42;
+
+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(100))
+        .open()?;
+
+    let now = SystemTime::now();
+
+    let io = DynamixelSerialIO::v1();
+
+    // Ping
+    let x = io.ping(serial_port.as_mut(), id);
+    println!("Ping : {:?}", x);
+
+    let mut t = now.elapsed().unwrap().as_secs_f32();
+    loop {
+        if t > 10.0 {
+            break;
+        }
+
+        // let x = io.ping(serial_port.as_mut(), id);
+        // println!("Ping : {:?}", x);
+
+        t = now.elapsed().unwrap().as_secs_f32();
+        let target = amplitude * 180.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin();
+
+        let pos = poulpe::read_motor_a_present_position(&io, serial_port.as_mut(), id)?;
+        thread::sleep(Duration::from_micros(1000));
+        // let target = poulpe::read_motor_a_goal_position(&io, serial_port.as_mut(), id)?;
+        let _ = poulpe::write_motor_a_goal_position(&io, serial_port.as_mut(), id, target as i32);
+        println!("target: {} pos: {}", target, pos);
+
+        thread::sleep(Duration::from_millis(1));
+    }
+
+    // orbita2dof_foc::write_torque_enable(&io, serial_port.as_mut(), id, 0x00)?;
+    // let reg = orbita2dof_foc::read_torque_enable(&io, serial_port.as_mut(), id)?;
+    // if reg == 0x01 {
+    //     println!("Motor on");
+    // } else {
+    //     println!("Motor off");
+    // }
+
+    thread::sleep(Duration::from_millis(2000));
+
+    Ok(())
+}

src/device/mod.rs

@@ -91,4 +91,5 @@ pub mod l0_force_fan;
 pub mod mx;
 pub mod orbita2dof_foc;
 pub mod orbita_foc;
+pub mod poulpe;
 pub mod xl320;

src/device/orbita_foc.rs

@@ -38,7 +38,8 @@ reg_read_only!(firmware_version, 6, u8);
 reg_read_write!(id, 7, u8);
 
 reg_write_only!(system_check, 8, u8);
-reg_read_only!(motors_drivers_states, 9, u8);
+
+reg_read_only!(motors_drivers_states, 159, u8);
 
 reg_read_write!(voltage_limit, 10, f32);
 // reg_read_write!(intensity_limit, 14, f32);
@@ -70,6 +71,11 @@ reg_read_only!(present_position, 71, DiskValue::<f32>);
 reg_read_only!(top_present_position, 71, f32);
 reg_read_only!(middle_present_position, 75, f32);
 reg_read_only!(bottom_present_position, 79, f32);
+
+reg_read_only!(top_present_hall, 160, f32);
+reg_read_only!(middle_present_hall, 164, f32);
+reg_read_only!(bottom_present_hall, 168, f32);
+
 // reg_read_only!(present_speed, 83, DiskValue::<f32>);
 // reg_read_only!(top_present_speed, 83, f32);
 // reg_read_only!(middle_present_speed, 87, f32);

src/protocol/v1.rs

@@ -148,6 +148,11 @@ impl StatusPacket<PacketV1> for StatusPacketV1 {
         let read_crc = *data.last().unwrap();
         let computed_crc = crc(&data[2..data.len() - 1]);
         if read_crc != computed_crc {
+            println!(
+                "read crc: {}, computed crc: {} data: {:?}",
+                read_crc, computed_crc, data
+            );
+
             return Err(Box::new(CommunicationErrorKind::ChecksumError));
         }