From 5706dab01027fb2756f61753473e93f3d440fc03 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=89tienne?= Date: Mon, 17 Apr 2023 10:13:39 +0200 Subject: [PATCH 01/52] Running quick and dirty exemple of Orbita 2 DoF (Elbow) with FSESC6.7 board running SimpleFOC + Dynamixel2Arduino libraries. --- examples/foc_2dof.rs | 118 ++++++++++++++++++++++ src/device/mod.rs | 1 + src/device/orbita2dof_foc.rs | 184 +++++++++++++++++++++++++++++++++++ src/device/orbita_foc.rs | 2 +- 4 files changed, 304 insertions(+), 1 deletion(-) create mode 100644 examples/foc_2dof.rs create mode 100644 src/device/orbita2dof_foc.rs diff --git a/examples/foc_2dof.rs b/examples/foc_2dof.rs new file mode 100644 index 0000000..7867321 --- /dev/null +++ b/examples/foc_2dof.rs @@ -0,0 +1,118 @@ +use std::f32::consts::PI; +use std::time::SystemTime; +use std::{error::Error, thread, time::Duration}; + +use rustypot::device::orbita2dof_foc::{self, DiskValue}; +use rustypot::DynamixelSerialIO; + +fn main() -> Result<(), Box> { + // Motor A + let mut serial_port_motor_a = serialport::new("/dev/ttyACM0", 1_000_000) + .timeout(Duration::from_millis(2000)) + .open()?; + + let io = DynamixelSerialIO::v1(); + + let id_motor_a = 71; + + let now = SystemTime::now(); + let x = io.ping(serial_port_motor_a.as_mut(), id_motor_a); + println!("{:?}", x); + + let reg = orbita2dof_foc::read_model_number(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + println!("model_number: {:#04x}", reg); + let reg = orbita2dof_foc::read_firmware_version(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + println!("firmware_version: {:#04x}", reg); + let reg = orbita2dof_foc::read_id(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + println!("id: {:?}", reg); + let reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + println!("voltage_limit: {:?}", reg); + + let reg = orbita2dof_foc::read_motors_drivers_states(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + println!("Motors/Drivers states: {:#010b}", reg); + + let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_motor_a.as_mut(), id_motor_a, 0x01)?; + let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + if reg == 0x01 { + println!("Motors started... ({:#04x})", reg); + } else { + println!(":-("); + } + + // Motor B + let mut serial_port_motor_b = serialport::new("/dev/ttyACM1", 1_000_000) + .timeout(Duration::from_millis(2000)) + .open()?; + + let io = DynamixelSerialIO::v1(); + + let id_motor_b = 72; + + let now = SystemTime::now(); + let x = io.ping(serial_port_motor_b.as_mut(), id_motor_b); + println!("{:?}", x); + + let reg = orbita2dof_foc::read_model_number(&io, serial_port_motor_b.as_mut(), id_motor_b)?; + println!("model_number: {:#04x}", reg); + let reg = orbita2dof_foc::read_firmware_version(&io, serial_port_motor_b.as_mut(), id_motor_b)?; + println!("firmware_version: {:#04x}", reg); + let reg = orbita2dof_foc::read_id(&io, serial_port_motor_b.as_mut(), id_motor_b)?; + println!("id: {:?}", reg); + let reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor_b.as_mut(), id_motor_b)?; + println!("voltage_limit: {:?}", reg); + + let reg = orbita2dof_foc::read_motors_drivers_states(&io, serial_port_motor_b.as_mut(), id_motor_b)?; + println!("Motor/Driver states: {:#010b}", reg); + + let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_motor_b.as_mut(), id_motor_b, 0x01)?; + let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_motor_b.as_mut(), id_motor_b)?; + if reg == 0x01 { + println!("Motor started... ({:#04x})", reg); + } else { + println!(":-("); + } + + loop { + let t = now.elapsed().unwrap().as_secs_f32(); + let target = 23.0_f32 * 180.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin(); + orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor_a.as_mut(), id_motor_a, target)?; + orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor_b.as_mut(), id_motor_b, -1.0_f32*target)?; + +/* let w_pos = orbita2dof_foc::read_motor_a_goal_position(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + let r_pos = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + print!("A[t: {:?} - a: {:?}]", w_pos, r_pos);*/ + let w_pos = orbita2dof_foc::read_motor_a_goal_position(&io, serial_port_motor_b.as_mut(), id_motor_b)?; + let r_pos = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor_b.as_mut(), id_motor_b)?; + print!(" - B[t: {:?} - a: {:?}]", w_pos, r_pos); + + println!(""); + thread::sleep(Duration::from_millis(10)); + } + + /* +// orbita_foc::write_system_check(&io, serial_port_motor_a.as_mut(), id, 0b00000001)?; + + let pos = orbita_foc::read_present_position(&io, serial_port_motor_a.as_mut(), id)?; + println!("{:?}", pos); + + loop { + let t = now.elapsed().unwrap().as_secs_f32(); + let target = 4.267_f32*180.0_f32.to_radians() * (2.0 * PI * 0.1 * t).sin(); + orbita_foc::write_top_goal_position(&io, serial_port_motor_a.as_mut(), id, target)?; + // println!("{}", t); + + orbita_foc::write_goal_position( + &io, + serial_port_motor_a.as_mut(), + id, + DiskValue { + top: pos.top + target, + middle: pos.middle + target, + bottom: pos.bottom + target, + }, + )?; + + thread::sleep(Duration::from_millis(10)); + }*/ +} + diff --git a/src/device/mod.rs b/src/device/mod.rs index d81d531..bb6b4b6 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -90,4 +90,5 @@ macro_rules! reg_read_write { pub mod l0_force_fan; pub mod mx; pub mod orbita_foc; +pub mod orbita2dof_foc; pub mod xl320; diff --git a/src/device/orbita2dof_foc.rs b/src/device/orbita2dof_foc.rs new file mode 100644 index 0000000..5ab34a5 --- /dev/null +++ b/src/device/orbita2dof_foc.rs @@ -0,0 +1,184 @@ +//! Orbita 2DoF Serial SimpleFOC register (protocol v1) + +use crate::device::*; + +/// Wrapper for a value per disk (top, middle, bottom) +#[derive(Clone, Copy, Debug)] +pub struct DiskValue { + pub a: T, + pub b: T, +} + +/// Wrapper for a 3D vector (x, y, z) +#[derive(Clone, Copy, Debug)] +pub struct Vec3d { + pub x: T, + pub y: T, + pub z: T, +} + +/// Wrapper for a Position/Speed/Load value for each disk +#[derive(Clone, Copy, Debug)] +pub struct DiskPositionSpeedLoad { + pub position: DiskValue, + pub speed: DiskValue, + pub load: DiskValue, +} +/// Wrapper for PID gains. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Pid { + pub p: f32, + pub i: f32, + pub d: f32, +} + +reg_read_only!(model_number, 0, u16); +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_write!(voltage_limit, 10, f32); +reg_read_write!(intensity_limit, 14, f32); + +reg_read_write!(velocity_pid, 18, Pid); +reg_read_write!(velocity_p_gain, 18, f32); +reg_read_write!(velocity_i_gain, 22, f32); +reg_read_write!(velocity_d_gain, 26, f32); +reg_read_write!(velocity_out_ramp, 30, f32); +reg_read_write!(velocity_low_pass_filter, 34, f32); + +reg_read_write!(angle_pid, 38, Pid); +reg_read_write!(angle_p_gain, 38, f32); +reg_read_write!(angle_i_gain, 42, f32); +reg_read_write!(angle_d_gain, 46, f32); +reg_read_write!(angle_velocity_limit, 50, f32); + +reg_read_write!(temperature_limit, 54, f32); + +reg_read_write!(torque_enable, 58, u8); + +//reg_read_write!(goal_position, 59, DiskValue::); +reg_read_write!(roll_goal_position, 59, f32); +reg_read_write!(pitch_goal_position, 63, f32); + +//reg_read_only!(present_position_speed_load, 67, DiskPositionSpeedLoad); +//reg_read_only!(present_position, 67, DiskValue::); +reg_read_only!(roll_present_position, 67, f32); +reg_read_only!(pitch_present_position, 71, f32); + +//reg_read_only!(motors_goal_position, 75, DiskValue::); +reg_read_write!(motor_a_goal_position, 75, f32); +reg_read_write!(motor_b_goal_position, 79, f32); +//reg_read_only!(motors_present_position, 83, DiskValue::); +reg_read_only!(motor_a_present_position, 83, f32); +reg_read_only!(motor_b_present_position, 87, f32); +//reg_read_only!(motors_present_velocity, 91, DiskValue::); +reg_read_only!(motor_a_present_velocity, 91, f32); +reg_read_only!(motor_b_present_velocity, 95, f32); +//reg_read_only!(motors_present_load, 99, DiskValue::); +reg_read_only!(motor_a_present_load, 99, f32); +reg_read_only!(motor_b_present_load, 103, f32); + +//reg_read_only!(present_temperature, 107, DiskValue::); +reg_read_only!(motor_a_present_temperature, 107, f32); +reg_read_only!(motor_b_present_temperature, 111, f32); + +reg_read_only!(imu_acc, 119, Vec3d::); +reg_read_only!(imu_acc_x, 119, f32); +reg_read_only!(imu_acc_y, 123, f32); +reg_read_only!(imu_acc_z, 127, f32); +reg_read_only!(imu_gyro, 131, Vec3d::); +reg_read_only!(imu_gyro_x, 131, f32); +reg_read_only!(imu_gyro_y, 135, f32); +reg_read_only!(imu_gyro_z, 139, f32); +reg_read_only!(imu_temperature, 143, f32); +/* +impl PartialEq for DiskValue { + fn eq(&self, other: &Self) -> bool { + self.top == other.top && self.middle == other.middle && self.bottom == other.bottom + } +} + +impl DiskValue { + pub fn from_le_bytes(bytes: [u8; 12]) -> Self { + DiskValue { + top: f32::from_le_bytes(bytes[0..4].try_into().unwrap()), + middle: f32::from_le_bytes(bytes[4..8].try_into().unwrap()), + bottom: f32::from_le_bytes(bytes[8..12].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 12] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.top.to_le_bytes()); + bytes.extend_from_slice(&self.middle.to_le_bytes()); + bytes.extend_from_slice(&self.bottom.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + +impl PartialEq for Vec3d { + fn eq(&self, other: &Self) -> bool { + self.x == other.x && self.y == other.y && self.z == other.z + } +}*/ + +impl Vec3d { + pub fn from_le_bytes(bytes: [u8; 12]) -> Self { + Vec3d { + x: f32::from_le_bytes(bytes[0..4].try_into().unwrap()), + y: f32::from_le_bytes(bytes[4..8].try_into().unwrap()), + z: f32::from_le_bytes(bytes[8..12].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 12] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.x.to_le_bytes()); + bytes.extend_from_slice(&self.y.to_le_bytes()); + bytes.extend_from_slice(&self.z.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + +impl Pid { + pub fn from_le_bytes(bytes: [u8; 12]) -> Self { + Pid { + p: f32::from_le_bytes(bytes[0..4].try_into().unwrap()), + i: f32::from_le_bytes(bytes[4..8].try_into().unwrap()), + d: f32::from_le_bytes(bytes[8..12].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 12] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.p.to_le_bytes()); + bytes.extend_from_slice(&self.i.to_le_bytes()); + bytes.extend_from_slice(&self.d.to_le_bytes()); + + bytes.try_into().unwrap() + } +} +/* +impl DiskPositionSpeedLoad { + pub fn from_le_bytes(bytes: [u8; 36]) -> Self { + DiskPositionSpeedLoad { + position: DiskValue::from_le_bytes(bytes[0..12].try_into().unwrap()), + speed: DiskValue::from_le_bytes(bytes[12..24].try_into().unwrap()), + load: DiskValue::from_le_bytes(bytes[24..36].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 36] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.position.to_le_bytes()); + bytes.extend_from_slice(&self.speed.to_le_bytes()); + bytes.extend_from_slice(&self.load.to_le_bytes()); + + bytes.try_into().unwrap() + } +}*/ diff --git a/src/device/orbita_foc.rs b/src/device/orbita_foc.rs index 151c1ec..4480513 100644 --- a/src/device/orbita_foc.rs +++ b/src/device/orbita_foc.rs @@ -1,4 +1,4 @@ -//! Orbita Serial SimpleFOC register (protocol v1) +//! Orbita 3DoF Serial SimpleFOC register (protocol v1) use crate::device::*; From 18929f3d838aeacddfbf7ecc1004eeb7bf629aca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=89tienne?= Date: Mon, 17 Apr 2023 16:51:48 +0200 Subject: [PATCH 02/52] DiskValue for Orbita2dof. --- examples/foc_2dof.rs | 32 +++++--------------------------- src/device/orbita2dof_foc.rs | 20 +++++++++----------- 2 files changed, 14 insertions(+), 38 deletions(-) diff --git a/examples/foc_2dof.rs b/examples/foc_2dof.rs index 7867321..da2c94e 100644 --- a/examples/foc_2dof.rs +++ b/examples/foc_2dof.rs @@ -25,6 +25,7 @@ fn main() -> Result<(), Box> { println!("firmware_version: {:#04x}", reg); let reg = orbita2dof_foc::read_id(&io, serial_port_motor_a.as_mut(), id_motor_a)?; println!("id: {:?}", reg); + let reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a, 5.0)?; let reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a)?; println!("voltage_limit: {:?}", reg); @@ -58,6 +59,7 @@ fn main() -> Result<(), Box> { println!("firmware_version: {:#04x}", reg); let reg = orbita2dof_foc::read_id(&io, serial_port_motor_b.as_mut(), id_motor_b)?; println!("id: {:?}", reg); + let reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_motor_b.as_mut(), id_motor_b, 1.1)?; let reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor_b.as_mut(), id_motor_b)?; println!("voltage_limit: {:?}", reg); @@ -83,36 +85,12 @@ fn main() -> Result<(), Box> { print!("A[t: {:?} - a: {:?}]", w_pos, r_pos);*/ let w_pos = orbita2dof_foc::read_motor_a_goal_position(&io, serial_port_motor_b.as_mut(), id_motor_b)?; let r_pos = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor_b.as_mut(), id_motor_b)?; - print!(" - B[t: {:?} - a: {:?}]", w_pos, r_pos); + let v_lim = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + print!("B[t: {:?} - a: {:?} - v_lim: {:?}]", w_pos, r_pos, v_lim); println!(""); thread::sleep(Duration::from_millis(10)); } - - /* -// orbita_foc::write_system_check(&io, serial_port_motor_a.as_mut(), id, 0b00000001)?; - - let pos = orbita_foc::read_present_position(&io, serial_port_motor_a.as_mut(), id)?; - println!("{:?}", pos); - - loop { - let t = now.elapsed().unwrap().as_secs_f32(); - let target = 4.267_f32*180.0_f32.to_radians() * (2.0 * PI * 0.1 * t).sin(); - orbita_foc::write_top_goal_position(&io, serial_port_motor_a.as_mut(), id, target)?; - // println!("{}", t); - - orbita_foc::write_goal_position( - &io, - serial_port_motor_a.as_mut(), - id, - DiskValue { - top: pos.top + target, - middle: pos.middle + target, - bottom: pos.bottom + target, - }, - )?; - - thread::sleep(Duration::from_millis(10)); - }*/ } + diff --git a/src/device/orbita2dof_foc.rs b/src/device/orbita2dof_foc.rs index 5ab34a5..926dd7b 100644 --- a/src/device/orbita2dof_foc.rs +++ b/src/device/orbita2dof_foc.rs @@ -94,27 +94,25 @@ reg_read_only!(imu_gyro_x, 131, f32); reg_read_only!(imu_gyro_y, 135, f32); reg_read_only!(imu_gyro_z, 139, f32); reg_read_only!(imu_temperature, 143, f32); -/* + impl PartialEq for DiskValue { fn eq(&self, other: &Self) -> bool { - self.top == other.top && self.middle == other.middle && self.bottom == other.bottom + self.a == other.a && self.b == other.b } } impl DiskValue { - pub fn from_le_bytes(bytes: [u8; 12]) -> Self { + pub fn from_le_bytes(bytes: [u8; 8]) -> Self { DiskValue { - top: f32::from_le_bytes(bytes[0..4].try_into().unwrap()), - middle: f32::from_le_bytes(bytes[4..8].try_into().unwrap()), - bottom: f32::from_le_bytes(bytes[8..12].try_into().unwrap()), + a: f32::from_le_bytes(bytes[0..4].try_into().unwrap()), + b: f32::from_le_bytes(bytes[4..8].try_into().unwrap()), } } - pub fn to_le_bytes(&self) -> [u8; 12] { + pub fn to_le_bytes(&self) -> [u8; 8] { let mut bytes = Vec::new(); - bytes.extend_from_slice(&self.top.to_le_bytes()); - bytes.extend_from_slice(&self.middle.to_le_bytes()); - bytes.extend_from_slice(&self.bottom.to_le_bytes()); + bytes.extend_from_slice(&self.a.to_le_bytes()); + bytes.extend_from_slice(&self.b.to_le_bytes()); bytes.try_into().unwrap() } @@ -124,7 +122,7 @@ impl PartialEq for Vec3d { fn eq(&self, other: &Self) -> bool { self.x == other.x && self.y == other.y && self.z == other.z } -}*/ +} impl Vec3d { pub fn from_le_bytes(bytes: [u8; 12]) -> Self { From fc7d993f17cf1b38043435c4a2d614e152a92e9a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=89tienne?= Date: Fri, 12 May 2023 14:53:40 +0200 Subject: [PATCH 03/52] Now setting voltage limit and getting load (voltage.q). --- examples/foc_2dof.rs | 68 ++++++++++++++++++++++++++++++++------------ 1 file changed, 50 insertions(+), 18 deletions(-) diff --git a/examples/foc_2dof.rs b/examples/foc_2dof.rs index da2c94e..049c9cb 100644 --- a/examples/foc_2dof.rs +++ b/examples/foc_2dof.rs @@ -2,13 +2,16 @@ use std::f32::consts::PI; use std::time::SystemTime; use std::{error::Error, thread, time::Duration}; -use rustypot::device::orbita2dof_foc::{self, DiskValue}; +use rustypot::device::orbita2dof_foc::{self};//, DiskValue}; use rustypot::DynamixelSerialIO; + fn main() -> Result<(), Box> { + println!("Hi there! [foc_2dof.rs]"); + // Motor A - let mut serial_port_motor_a = serialport::new("/dev/ttyACM0", 1_000_000) - .timeout(Duration::from_millis(2000)) + let mut serial_port_motor_a = serialport::new("/dev/ttyACM1", 1_000_000) + .timeout(Duration::from_millis(100)) .open()?; let io = DynamixelSerialIO::v1(); @@ -17,18 +20,29 @@ fn main() -> Result<(), Box> { let now = SystemTime::now(); let x = io.ping(serial_port_motor_a.as_mut(), id_motor_a); - println!("{:?}", x); - + println!("Ping: {:?}", x); + let reg = orbita2dof_foc::read_id(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + println!("id: {:?}", reg); let reg = orbita2dof_foc::read_model_number(&io, serial_port_motor_a.as_mut(), id_motor_a)?; println!("model_number: {:#04x}", reg); let reg = orbita2dof_foc::read_firmware_version(&io, serial_port_motor_a.as_mut(), id_motor_a)?; println!("firmware_version: {:#04x}", reg); - let reg = orbita2dof_foc::read_id(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - println!("id: {:?}", reg); - let reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a, 5.0)?; + orbita2dof_foc::write_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a, 1.0)?; let reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a)?; println!("voltage_limit: {:?}", reg); + + let reg = orbita2dof_foc::read_velocity_p_gain(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + print!("Velocity - P [{:?}]", reg); + let reg = orbita2dof_foc::read_velocity_i_gain(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + print!(" I [{:?}]", reg); + let reg = orbita2dof_foc::read_velocity_d_gain(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + println!(" D [{:?}]", reg); + let reg = orbita2dof_foc::read_velocity_out_ramp(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + print!("Output Ramp [{:?}]", reg); + let reg = orbita2dof_foc::read_velocity_low_pass_filter(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + println!(" Low-Pass Filter [{:?}]", reg); + let reg = orbita2dof_foc::read_motors_drivers_states(&io, serial_port_motor_a.as_mut(), id_motor_a)?; println!("Motors/Drivers states: {:#010b}", reg); @@ -40,8 +54,14 @@ fn main() -> Result<(), Box> { println!(":-("); } +/*orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor_a.as_mut(), id_motor_a, 0.0_f32)?; +orbita2dof_foc::write_torque_enable(&io, serial_port_motor_a.as_mut(), id_motor_a, 0x00)?; +Ok(())*/ + + + // Motor B - let mut serial_port_motor_b = serialport::new("/dev/ttyACM1", 1_000_000) +/* let mut serial_port_motor_b = serialport::new("/dev/ttyACM1", 1_000_000) .timeout(Duration::from_millis(2000)) .open()?; @@ -72,23 +92,35 @@ fn main() -> Result<(), Box> { println!("Motor started... ({:#04x})", reg); } else { println!(":-("); - } + }*/ + let mut display_counter = 0; loop { let t = now.elapsed().unwrap().as_secs_f32(); let target = 23.0_f32 * 180.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin(); orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor_a.as_mut(), id_motor_a, target)?; - orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor_b.as_mut(), id_motor_b, -1.0_f32*target)?; +// orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor_b.as_mut(), id_motor_b, -1.0_f32*target)?; -/* let w_pos = orbita2dof_foc::read_motor_a_goal_position(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + let w_pos = orbita2dof_foc::read_motor_a_goal_position(&io, serial_port_motor_a.as_mut(), id_motor_a)?; let r_pos = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - print!("A[t: {:?} - a: {:?}]", w_pos, r_pos);*/ - let w_pos = orbita2dof_foc::read_motor_a_goal_position(&io, serial_port_motor_b.as_mut(), id_motor_b)?; - let r_pos = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor_b.as_mut(), id_motor_b)?; - let v_lim = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - print!("B[t: {:?} - a: {:?} - v_lim: {:?}]", w_pos, r_pos, v_lim); + let r_vel = orbita2dof_foc::read_motor_a_present_velocity(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + let r_ld = orbita2dof_foc::read_motor_a_present_load(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + if display_counter == 0 { + print!("A[t: {:>5.1?} - a: {:>5.1?} - v: {:>5.1?} - l: {:>5.1?}]", w_pos, r_pos, r_vel, r_ld); + } +// let w_pos = orbita2dof_foc::read_motor_a_goal_position(&io, serial_port_motor_b.as_mut(), id_motor_b)?; +// let r_pos = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor_b.as_mut(), id_motor_b)?; +// let v_lim = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a)?; +// print!("B[t: {:?} - a: {:?} - v_lim: {:?}]", w_pos, r_pos, v_lim); - println!(""); + if display_counter == 0 { + println!(""); + } + display_counter += 1; + if display_counter > 10 { + display_counter = 0; + } + thread::sleep(Duration::from_millis(10)); } } From 0caa66e41d54a35c91ac9cd84e89fc2dba4ed99a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=89tienne?= Date: Thu, 8 Jun 2023 10:23:03 +0200 Subject: [PATCH 04/52] Running example on Reachy 2 prototype: slow fitness movement. --- examples/foc.rs | 7 +- examples/foc_2dof.rs | 190 +++++++++++++++++++++-------------- src/device/orbita2dof_foc.rs | 4 +- src/device/orbita_foc.rs | 4 + 4 files changed, 123 insertions(+), 82 deletions(-) diff --git a/examples/foc.rs b/examples/foc.rs index 59e873d..757851b 100644 --- a/examples/foc.rs +++ b/examples/foc.rs @@ -7,12 +7,15 @@ use rustypot::DynamixelSerialIO; fn main() -> Result<(), Box> { let mut serial_port = serialport::new("/dev/ttyUSB0", 1_000_000) - .timeout(Duration::from_millis(20)) + .timeout(Duration::from_millis(100)) .open()?; let io = DynamixelSerialIO::v1(); - let id = 42; + let id = 70; + orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 0); + + thread::sleep(Duration::from_millis(100)); let now = SystemTime::now(); // let x = io.ping(serial_port.as_mut(), id); diff --git a/examples/foc_2dof.rs b/examples/foc_2dof.rs index 049c9cb..94d3204 100644 --- a/examples/foc_2dof.rs +++ b/examples/foc_2dof.rs @@ -2,116 +2,150 @@ use std::f32::consts::PI; use std::time::SystemTime; use std::{error::Error, thread, time::Duration}; -use rustypot::device::orbita2dof_foc::{self};//, DiskValue}; +use rustypot::device::orbita2dof_foc::{self}; +use rustypot::device::orbita_foc::{self, DiskValue}; use rustypot::DynamixelSerialIO; +const RIGHT_ARM_WRIST: u8 = 70; +const RIGHT_ARM_ELBOW_MOTOR_A: u8 = 71; +const RIGHT_ARM_ELBOW_MOTOR_B: u8 = 72; +const RIGHT_ARM_SHOULDER_MOTOR_A: u8 = 81; +const RIGHT_ARM_SHOULDER_MOTOR_B: u8 = 82; fn main() -> Result<(), Box> { println!("Hi there! [foc_2dof.rs]"); - // Motor A - let mut serial_port_motor_a = serialport::new("/dev/ttyACM1", 1_000_000) + let mut serial_port_right_shoulder_motor_a = serialport::new("/dev/right_shoulder_A", 1_000_000) .timeout(Duration::from_millis(100)) .open()?; - let io = DynamixelSerialIO::v1(); + let mut serial_port_right_shoulder_motor_b = serialport::new("/dev/right_shoulder_B", 1_000_000) + .timeout(Duration::from_millis(100)) + .open()?; - let id_motor_a = 71; + let mut serial_port_right_elbow_motor_a = serialport::new("/dev/right_elbow_A", 1_000_000) + .timeout(Duration::from_millis(100)) + .open()?; - let now = SystemTime::now(); - let x = io.ping(serial_port_motor_a.as_mut(), id_motor_a); - println!("Ping: {:?}", x); - let reg = orbita2dof_foc::read_id(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - println!("id: {:?}", reg); - let reg = orbita2dof_foc::read_model_number(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - println!("model_number: {:#04x}", reg); - let reg = orbita2dof_foc::read_firmware_version(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - println!("firmware_version: {:#04x}", reg); - orbita2dof_foc::write_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a, 1.0)?; - let reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - println!("voltage_limit: {:?}", reg); - + let mut serial_port_right_elbow_motor_b = serialport::new("/dev/right_elbow_B", 1_000_000) + .timeout(Duration::from_millis(100)) + .open()?; + + let mut serial_port_right_wrist = serialport::new("/dev/right_wrist", 1_000_000) + .timeout(Duration::from_millis(100)) + .open()?; - let reg = orbita2dof_foc::read_velocity_p_gain(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - print!("Velocity - P [{:?}]", reg); - let reg = orbita2dof_foc::read_velocity_i_gain(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - print!(" I [{:?}]", reg); - let reg = orbita2dof_foc::read_velocity_d_gain(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - println!(" D [{:?}]", reg); - let reg = orbita2dof_foc::read_velocity_out_ramp(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - print!("Output Ramp [{:?}]", reg); - let reg = orbita2dof_foc::read_velocity_low_pass_filter(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - println!(" Low-Pass Filter [{:?}]", reg); + let now = SystemTime::now(); + + let io = DynamixelSerialIO::v1(); - let reg = orbita2dof_foc::read_motors_drivers_states(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - println!("Motors/Drivers states: {:#010b}", reg); + let id_right_shoulder_motor_a = RIGHT_ARM_SHOULDER_MOTOR_A; + let id_right_shoulder_motor_b = RIGHT_ARM_SHOULDER_MOTOR_B; + let id_right_elbow_motor_a = RIGHT_ARM_ELBOW_MOTOR_A; + let id_right_elbow_motor_b = RIGHT_ARM_ELBOW_MOTOR_B; + let id_right_wrist = RIGHT_ARM_WRIST; - let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_motor_a.as_mut(), id_motor_a, 0x01)?; - let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_motor_a.as_mut(), id_motor_a)?; + // Ping + let x = io.ping(serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a); + println!("Ping (id_right_shoulder_motor_a): {:?}", x); + let x = io.ping(serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b); + println!("Ping (id_right_shoulder_motor_b): {:?}", x); + let x = io.ping(serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a); + println!("Ping (id_right_elbow_motor_a): {:?}", x); + let x = io.ping(serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b); + println!("Ping (id_right_elbow_motor_b): {:?}", x); + let x = io.ping(serial_port_right_wrist.as_mut(), id_right_wrist); + println!("Ping (id_right_wrist): {:?}", x); + + // Set power + let _reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a, 10.0)?; + let _reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b, 10.0)?; + let _reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a, 10.0)?; + let _reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b, 10.0)?; + let _reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a)?; + print!("v_limit {:>5.3?} -", _reg); + let _reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b)?; + print!("v_limit {:>5.3?} -", _reg); + let _reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a)?; + print!("v_limit {:>5.3?} -", _reg); + let _reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b)?; + print!("v_limit {:>5.3?} -", _reg); + println!(""); + + + // Torque enable/disable + let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a, 0x01)?; + let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a)?; if reg == 0x01 { - println!("Motors started... ({:#04x})", reg); + println!("Shoulder - Motor A started... ({:#04x})", reg); } else { println!(":-("); } - -/*orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor_a.as_mut(), id_motor_a, 0.0_f32)?; -orbita2dof_foc::write_torque_enable(&io, serial_port_motor_a.as_mut(), id_motor_a, 0x00)?; -Ok(())*/ - + let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b, 0x01)?; + let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b)?; + if reg == 0x01 { + println!("Shoulder - Motor B started... ({:#04x})", reg); + } else { + println!(":-("); + } - // Motor B -/* let mut serial_port_motor_b = serialport::new("/dev/ttyACM1", 1_000_000) - .timeout(Duration::from_millis(2000)) - .open()?; - - let io = DynamixelSerialIO::v1(); - - let id_motor_b = 72; + let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a, 0x01)?; + let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a)?; + if reg == 0x01 { + println!("Elbow - Motor A started... ({:#04x})", reg); + } else { + println!(":-("); + } - let now = SystemTime::now(); - let x = io.ping(serial_port_motor_b.as_mut(), id_motor_b); - println!("{:?}", x); - - let reg = orbita2dof_foc::read_model_number(&io, serial_port_motor_b.as_mut(), id_motor_b)?; - println!("model_number: {:#04x}", reg); - let reg = orbita2dof_foc::read_firmware_version(&io, serial_port_motor_b.as_mut(), id_motor_b)?; - println!("firmware_version: {:#04x}", reg); - let reg = orbita2dof_foc::read_id(&io, serial_port_motor_b.as_mut(), id_motor_b)?; - println!("id: {:?}", reg); - let reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_motor_b.as_mut(), id_motor_b, 1.1)?; - let reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor_b.as_mut(), id_motor_b)?; - println!("voltage_limit: {:?}", reg); - - let reg = orbita2dof_foc::read_motors_drivers_states(&io, serial_port_motor_b.as_mut(), id_motor_b)?; - println!("Motor/Driver states: {:#010b}", reg); + let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b, 0x01)?; + let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b)?; + if reg == 0x01 { + println!("Elbow - Motor B started... ({:#04x})", reg); + } else { + println!(":-("); + } - let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_motor_b.as_mut(), id_motor_b, 0x01)?; - let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_motor_b.as_mut(), id_motor_b)?; + let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist, 0x01)?; + let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist)?; if reg == 0x01 { - println!("Motor started... ({:#04x})", reg); + println!("Wrist started... ({:#04x})", reg); } else { println!(":-("); - }*/ + } let mut display_counter = 0; loop { let t = now.elapsed().unwrap().as_secs_f32(); - let target = 23.0_f32 * 180.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin(); - orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor_a.as_mut(), id_motor_a, target)?; -// orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor_b.as_mut(), id_motor_b, -1.0_f32*target)?; + let target = 10.0_f32 * 180.0_f32.to_radians() * (2.0 * PI * 0.1 * t).sin(); + + orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a, target)?; + orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b, 1.0_f32*target)?; + orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a, -1.0_f32*target)?; + orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b, 1.0_f32*target)?; + + let target_wrist = 60.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin(); + +/* orbita_foc::write_goal_position( + &io, + serial_port_right_wrist.as_mut(), + id_right_wrist, + DiskValue { + top: target_wrist, + middle: target_wrist, + bottom: target_wrist, + }, + )?;*/ - let w_pos = orbita2dof_foc::read_motor_a_goal_position(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - let r_pos = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - let r_vel = orbita2dof_foc::read_motor_a_present_velocity(&io, serial_port_motor_a.as_mut(), id_motor_a)?; - let r_ld = orbita2dof_foc::read_motor_a_present_load(&io, serial_port_motor_a.as_mut(), id_motor_a)?; if display_counter == 0 { - print!("A[t: {:>5.1?} - a: {:>5.1?} - v: {:>5.1?} - l: {:>5.1?}]", w_pos, r_pos, r_vel, r_ld); + let shoulder_ring_pos = orbita2dof_foc::read_sensor_ring_present_position(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a)?; + let shoulder_center_pos = orbita2dof_foc::read_sensor_center_present_position(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b)?; + let elbow_ring_pos = orbita2dof_foc::read_sensor_ring_present_position(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a)?; + let elbow_center_pos = orbita2dof_foc::read_sensor_center_present_position(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b)?; + + print!("[Shoulder Ring_s {:>5.3?} - Center_s: {:>5.3?}] -", shoulder_ring_pos, shoulder_center_pos); + print!("[Elbow Ring_s {:>5.3?} - Center_s: {:>5.3?}] -", elbow_ring_pos, elbow_center_pos); } -// let w_pos = orbita2dof_foc::read_motor_a_goal_position(&io, serial_port_motor_b.as_mut(), id_motor_b)?; -// let r_pos = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor_b.as_mut(), id_motor_b)?; -// let v_lim = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor_a.as_mut(), id_motor_a)?; -// print!("B[t: {:?} - a: {:?} - v_lim: {:?}]", w_pos, r_pos, v_lim); if display_counter == 0 { println!(""); diff --git a/src/device/orbita2dof_foc.rs b/src/device/orbita2dof_foc.rs index 926dd7b..a4c4a9c 100644 --- a/src/device/orbita2dof_foc.rs +++ b/src/device/orbita2dof_foc.rs @@ -65,8 +65,8 @@ reg_read_write!(pitch_goal_position, 63, f32); //reg_read_only!(present_position_speed_load, 67, DiskPositionSpeedLoad); //reg_read_only!(present_position, 67, DiskValue::); -reg_read_only!(roll_present_position, 67, f32); -reg_read_only!(pitch_present_position, 71, f32); +reg_read_only!(sensor_ring_present_position, 67, f32); +reg_read_only!(sensor_center_present_position, 71, f32); //reg_read_only!(motors_goal_position, 75, DiskValue::); reg_read_write!(motor_a_goal_position, 75, f32); diff --git a/src/device/orbita_foc.rs b/src/device/orbita_foc.rs index 4480513..ab6aec3 100644 --- a/src/device/orbita_foc.rs +++ b/src/device/orbita_foc.rs @@ -94,6 +94,10 @@ reg_read_only!(bottom_present_position, 79, f32); // reg_read_only!(imu_gyro_z, 139, f32); // reg_read_only!(imu_temperature, 143, f32); +reg_read_only!(top_current_phase_a, 147, f32); +reg_read_only!(top_current_phase_b, 151, f32); +reg_read_only!(top_dc_current, 155, f32); + impl PartialEq for DiskValue { fn eq(&self, other: &Self) -> bool { self.top == other.top && self.middle == other.middle && self.bottom == other.bottom From fe08ef0113a42f77beac4a5f1507c31559ac3de4 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Thu, 8 Jun 2023 17:12:56 +0200 Subject: [PATCH 05/52] ?? --- examples/foc_2dof.rs | 306 ++++++++++++++++++++++++++++++------------- 1 file changed, 216 insertions(+), 90 deletions(-) diff --git a/examples/foc_2dof.rs b/examples/foc_2dof.rs index 94d3204..afd1855 100644 --- a/examples/foc_2dof.rs +++ b/examples/foc_2dof.rs @@ -6,22 +6,24 @@ use rustypot::device::orbita2dof_foc::{self}; use rustypot::device::orbita_foc::{self, DiskValue}; use rustypot::DynamixelSerialIO; -const RIGHT_ARM_WRIST: u8 = 70; -const RIGHT_ARM_ELBOW_MOTOR_A: u8 = 71; -const RIGHT_ARM_ELBOW_MOTOR_B: u8 = 72; +const RIGHT_ARM_WRIST: u8 = 70; +const RIGHT_ARM_ELBOW_MOTOR_A: u8 = 71; +const RIGHT_ARM_ELBOW_MOTOR_B: u8 = 72; const RIGHT_ARM_SHOULDER_MOTOR_A: u8 = 81; const RIGHT_ARM_SHOULDER_MOTOR_B: u8 = 82; fn main() -> Result<(), Box> { println!("Hi there! [foc_2dof.rs]"); - - let mut serial_port_right_shoulder_motor_a = serialport::new("/dev/right_shoulder_A", 1_000_000) - .timeout(Duration::from_millis(100)) - .open()?; - let mut serial_port_right_shoulder_motor_b = serialport::new("/dev/right_shoulder_B", 1_000_000) - .timeout(Duration::from_millis(100)) - .open()?; + let mut serial_port_right_shoulder_motor_a = + serialport::new("/dev/right_shoulder_A", 1_000_000) + .timeout(Duration::from_millis(100)) + .open()?; + + let mut serial_port_right_shoulder_motor_b = + serialport::new("/dev/right_shoulder_B", 1_000_000) + .timeout(Duration::from_millis(100)) + .open()?; let mut serial_port_right_elbow_motor_a = serialport::new("/dev/right_elbow_A", 1_000_000) .timeout(Duration::from_millis(100)) @@ -31,102 +33,206 @@ fn main() -> Result<(), Box> { .timeout(Duration::from_millis(100)) .open()?; - let mut serial_port_right_wrist = serialport::new("/dev/right_wrist", 1_000_000) - .timeout(Duration::from_millis(100)) - .open()?; + // let mut serial_port_right_wrist = serialport::new("/dev/right_wrist", 1_000_000) + // .timeout(Duration::from_millis(100)) + // .open()?; let now = SystemTime::now(); - + let io = DynamixelSerialIO::v1(); let id_right_shoulder_motor_a = RIGHT_ARM_SHOULDER_MOTOR_A; let id_right_shoulder_motor_b = RIGHT_ARM_SHOULDER_MOTOR_B; - let id_right_elbow_motor_a = RIGHT_ARM_ELBOW_MOTOR_A; - let id_right_elbow_motor_b = RIGHT_ARM_ELBOW_MOTOR_B; - let id_right_wrist = RIGHT_ARM_WRIST; - + let id_right_elbow_motor_a = RIGHT_ARM_ELBOW_MOTOR_A; + let id_right_elbow_motor_b = RIGHT_ARM_ELBOW_MOTOR_B; + let id_right_wrist = RIGHT_ARM_WRIST; + // Ping - let x = io.ping(serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a); + let x = io.ping( + serial_port_right_shoulder_motor_a.as_mut(), + id_right_shoulder_motor_a, + ); println!("Ping (id_right_shoulder_motor_a): {:?}", x); - let x = io.ping(serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b); + let x = io.ping( + serial_port_right_shoulder_motor_b.as_mut(), + id_right_shoulder_motor_b, + ); println!("Ping (id_right_shoulder_motor_b): {:?}", x); - let x = io.ping(serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a); + let x = io.ping( + serial_port_right_elbow_motor_a.as_mut(), + id_right_elbow_motor_a, + ); println!("Ping (id_right_elbow_motor_a): {:?}", x); - let x = io.ping(serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b); + let x = io.ping( + serial_port_right_elbow_motor_b.as_mut(), + id_right_elbow_motor_b, + ); println!("Ping (id_right_elbow_motor_b): {:?}", x); - let x = io.ping(serial_port_right_wrist.as_mut(), id_right_wrist); - println!("Ping (id_right_wrist): {:?}", x); - + + // let x = io.ping(serial_port_right_wrist.as_mut(), id_right_wrist); + // println!("Ping (id_right_wrist): {:?}", x); + // Set power - let _reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a, 10.0)?; - let _reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b, 10.0)?; - let _reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a, 10.0)?; - let _reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b, 10.0)?; - let _reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a)?; - print!("v_limit {:>5.3?} -", _reg); - let _reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b)?; - print!("v_limit {:>5.3?} -", _reg); - let _reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a)?; - print!("v_limit {:>5.3?} -", _reg); - let _reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b)?; - print!("v_limit {:>5.3?} -", _reg); - println!(""); - - + let _reg = orbita2dof_foc::write_voltage_limit( + &io, + serial_port_right_shoulder_motor_a.as_mut(), + id_right_shoulder_motor_a, + 10.0, + )?; + let _reg = orbita2dof_foc::write_voltage_limit( + &io, + serial_port_right_shoulder_motor_b.as_mut(), + id_right_shoulder_motor_b, + 10.0, + )?; + let _reg = orbita2dof_foc::write_voltage_limit( + &io, + serial_port_right_elbow_motor_a.as_mut(), + id_right_elbow_motor_a, + 10.0, + )?; + let _reg = orbita2dof_foc::write_voltage_limit( + &io, + serial_port_right_elbow_motor_b.as_mut(), + id_right_elbow_motor_b, + 10.0, + )?; + let _reg = orbita2dof_foc::read_voltage_limit( + &io, + serial_port_right_shoulder_motor_a.as_mut(), + id_right_shoulder_motor_a, + )?; + print!("v_limit {:>5.3?} -", _reg); + let _reg = orbita2dof_foc::read_voltage_limit( + &io, + serial_port_right_shoulder_motor_b.as_mut(), + id_right_shoulder_motor_b, + )?; + print!("v_limit {:>5.3?} -", _reg); + let _reg = orbita2dof_foc::read_voltage_limit( + &io, + serial_port_right_elbow_motor_a.as_mut(), + id_right_elbow_motor_a, + )?; + print!("v_limit {:>5.3?} -", _reg); + let _reg = orbita2dof_foc::read_voltage_limit( + &io, + serial_port_right_elbow_motor_b.as_mut(), + id_right_elbow_motor_b, + )?; + print!("v_limit {:>5.3?} -", _reg); + println!(""); + // Torque enable/disable - let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a, 0x01)?; - let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a)?; + let _reg = orbita2dof_foc::write_torque_enable( + &io, + serial_port_right_shoulder_motor_a.as_mut(), + id_right_shoulder_motor_a, + 0x01, + )?; + let reg = orbita2dof_foc::read_torque_enable( + &io, + serial_port_right_shoulder_motor_a.as_mut(), + id_right_shoulder_motor_a, + )?; if reg == 0x01 { - println!("Shoulder - Motor A started... ({:#04x})", reg); + println!("Shoulder - Motor A started... ({:#04x})", reg); } else { - println!(":-("); + println!(":-("); } - let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b, 0x01)?; - let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b)?; - if reg == 0x01 { - println!("Shoulder - Motor B started... ({:#04x})", reg); - } else { - println!(":-("); - } - - let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a, 0x01)?; - let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a)?; + let _reg = orbita2dof_foc::write_torque_enable( + &io, + serial_port_right_shoulder_motor_b.as_mut(), + id_right_shoulder_motor_b, + 0x01, + )?; + let reg = orbita2dof_foc::read_torque_enable( + &io, + serial_port_right_shoulder_motor_b.as_mut(), + id_right_shoulder_motor_b, + )?; if reg == 0x01 { - println!("Elbow - Motor A started... ({:#04x})", reg); + println!("Shoulder - Motor B started... ({:#04x})", reg); } else { - println!(":-("); + println!(":-("); } - let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b, 0x01)?; - let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b)?; + let _reg = orbita2dof_foc::write_torque_enable( + &io, + serial_port_right_elbow_motor_a.as_mut(), + id_right_elbow_motor_a, + 0x01, + )?; + let reg = orbita2dof_foc::read_torque_enable( + &io, + serial_port_right_elbow_motor_a.as_mut(), + id_right_elbow_motor_a, + )?; if reg == 0x01 { - println!("Elbow - Motor B started... ({:#04x})", reg); + println!("Elbow - Motor A started... ({:#04x})", reg); } else { - println!(":-("); + println!(":-("); } - - let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist, 0x01)?; - let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist)?; + + let _reg = orbita2dof_foc::write_torque_enable( + &io, + serial_port_right_elbow_motor_b.as_mut(), + id_right_elbow_motor_b, + 0x01, + )?; + let reg = orbita2dof_foc::read_torque_enable( + &io, + serial_port_right_elbow_motor_b.as_mut(), + id_right_elbow_motor_b, + )?; if reg == 0x01 { - println!("Wrist started... ({:#04x})", reg); + println!("Elbow - Motor B started... ({:#04x})", reg); } else { - println!(":-("); + println!(":-("); } - + + // let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist, 0x01)?; + // let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist)?; + // if reg == 0x01 { + // println!("Wrist started... ({:#04x})", reg); + // } else { + // println!(":-("); + // } + let mut display_counter = 0; loop { let t = now.elapsed().unwrap().as_secs_f32(); let target = 10.0_f32 * 180.0_f32.to_radians() * (2.0 * PI * 0.1 * t).sin(); - orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a, target)?; - orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b, 1.0_f32*target)?; - orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a, -1.0_f32*target)?; - orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b, 1.0_f32*target)?; - + orbita2dof_foc::write_motor_a_goal_position( + &io, + serial_port_right_shoulder_motor_a.as_mut(), + id_right_shoulder_motor_a, + target, + )?; + orbita2dof_foc::write_motor_a_goal_position( + &io, + serial_port_right_shoulder_motor_b.as_mut(), + id_right_shoulder_motor_b, + 1.0_f32 * target, + )?; + orbita2dof_foc::write_motor_a_goal_position( + &io, + serial_port_right_elbow_motor_a.as_mut(), + id_right_elbow_motor_a, + -1.0_f32 * target, + )?; + orbita2dof_foc::write_motor_a_goal_position( + &io, + serial_port_right_elbow_motor_b.as_mut(), + id_right_elbow_motor_b, + 1.0_f32 * target, + )?; + let target_wrist = 60.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin(); -/* orbita_foc::write_goal_position( + /* orbita_foc::write_goal_position( &io, serial_port_right_wrist.as_mut(), id_right_wrist, @@ -136,27 +242,47 @@ fn main() -> Result<(), Box> { bottom: target_wrist, }, )?;*/ - + if display_counter == 0 { - let shoulder_ring_pos = orbita2dof_foc::read_sensor_ring_present_position(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a)?; - let shoulder_center_pos = orbita2dof_foc::read_sensor_center_present_position(&io, serial_port_right_shoulder_motor_b.as_mut(), id_right_shoulder_motor_b)?; - let elbow_ring_pos = orbita2dof_foc::read_sensor_ring_present_position(&io, serial_port_right_elbow_motor_a.as_mut(), id_right_elbow_motor_a)?; - let elbow_center_pos = orbita2dof_foc::read_sensor_center_present_position(&io, serial_port_right_elbow_motor_b.as_mut(), id_right_elbow_motor_b)?; - - print!("[Shoulder Ring_s {:>5.3?} - Center_s: {:>5.3?}] -", shoulder_ring_pos, shoulder_center_pos); - print!("[Elbow Ring_s {:>5.3?} - Center_s: {:>5.3?}] -", elbow_ring_pos, elbow_center_pos); - } - + let shoulder_ring_pos = orbita2dof_foc::read_sensor_ring_present_position( + &io, + serial_port_right_shoulder_motor_a.as_mut(), + id_right_shoulder_motor_a, + )?; + let shoulder_center_pos = orbita2dof_foc::read_sensor_center_present_position( + &io, + serial_port_right_shoulder_motor_b.as_mut(), + id_right_shoulder_motor_b, + )?; + let elbow_ring_pos = orbita2dof_foc::read_sensor_ring_present_position( + &io, + serial_port_right_elbow_motor_a.as_mut(), + id_right_elbow_motor_a, + )?; + let elbow_center_pos = orbita2dof_foc::read_sensor_center_present_position( + &io, + serial_port_right_elbow_motor_b.as_mut(), + id_right_elbow_motor_b, + )?; + + print!( + "[Shoulder Ring_s {:>5.3?} - Center_s: {:>5.3?}] -", + shoulder_ring_pos, shoulder_center_pos + ); + print!( + "[Elbow Ring_s {:>5.3?} - Center_s: {:>5.3?}] -", + elbow_ring_pos, elbow_center_pos + ); + } + if display_counter == 0 { - println!(""); - } - display_counter += 1; + println!(""); + } + display_counter += 1; if display_counter > 10 { - display_counter = 0; - } - + display_counter = 0; + } + thread::sleep(Duration::from_millis(10)); } } - - From 630e45b348819dbfcb3c14f7bcf3436173d37094 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Fri, 9 Jun 2023 11:15:09 +0200 Subject: [PATCH 06/52] fix wrist --- examples/foc_2dof.rs | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/examples/foc_2dof.rs b/examples/foc_2dof.rs index afd1855..52adee7 100644 --- a/examples/foc_2dof.rs +++ b/examples/foc_2dof.rs @@ -33,9 +33,9 @@ fn main() -> Result<(), Box> { .timeout(Duration::from_millis(100)) .open()?; - // let mut serial_port_right_wrist = serialport::new("/dev/right_wrist", 1_000_000) - // .timeout(Duration::from_millis(100)) - // .open()?; + let mut serial_port_right_wrist = serialport::new("/dev/right_wrist", 1_000_000) + .timeout(Duration::from_millis(100)) + .open()?; let now = SystemTime::now(); @@ -69,8 +69,8 @@ fn main() -> Result<(), Box> { ); println!("Ping (id_right_elbow_motor_b): {:?}", x); - // let x = io.ping(serial_port_right_wrist.as_mut(), id_right_wrist); - // println!("Ping (id_right_wrist): {:?}", x); + let x = io.ping(serial_port_right_wrist.as_mut(), id_right_wrist); + println!("Ping (id_right_wrist): {:?}", x); // Set power let _reg = orbita2dof_foc::write_voltage_limit( @@ -192,6 +192,8 @@ fn main() -> Result<(), Box> { println!(":-("); } + orbita_foc::write_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist, 0x01)?; + // let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist, 0x01)?; // let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist)?; // if reg == 0x01 { @@ -231,17 +233,22 @@ fn main() -> Result<(), Box> { )?; let target_wrist = 60.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin(); + let disks = orbita_foc::read_present_position( + &io, + serial_port_right_wrist.as_mut(), + id_right_wrist, + )?; - /* orbita_foc::write_goal_position( + orbita_foc::write_goal_position( &io, serial_port_right_wrist.as_mut(), id_right_wrist, DiskValue { - top: target_wrist, - middle: target_wrist, - bottom: target_wrist, + top: disks.top + target_wrist, + middle: disks.middle + target_wrist, + bottom: disks.bottom + target_wrist, }, - )?;*/ + )?; if display_counter == 0 { let shoulder_ring_pos = orbita2dof_foc::read_sensor_ring_present_position( From 1318213b4f727863d0768778e17132e08ac168f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=89tienne?= Date: Thu, 15 Jun 2023 14:10:34 +0200 Subject: [PATCH 07/52] [added] read speed and currents. --- examples/foc_2dof.rs | 81 ++++++++++++++++++++++++++++++------ src/device/orbita2dof_foc.rs | 11 +++-- 2 files changed, 76 insertions(+), 16 deletions(-) diff --git a/examples/foc_2dof.rs b/examples/foc_2dof.rs index 94d3204..8e6d07e 100644 --- a/examples/foc_2dof.rs +++ b/examples/foc_2dof.rs @@ -15,7 +15,64 @@ const RIGHT_ARM_SHOULDER_MOTOR_B: u8 = 82; fn main() -> Result<(), Box> { println!("Hi there! [foc_2dof.rs]"); - let mut serial_port_right_shoulder_motor_a = serialport::new("/dev/right_shoulder_A", 1_000_000) + let mut serial_port_motor = serialport::new("/dev/tool-XT90-A", 1_000_000) + .timeout(Duration::from_millis(100)) + .open()?; + + let now = SystemTime::now(); + let io = DynamixelSerialIO::v1(); + let id_motor = RIGHT_ARM_SHOULDER_MOTOR_B; + + let x = io.ping(serial_port_motor.as_mut(), id_motor); + println!("Ping ({:?}): {:?}", id_motor, x); + + let _reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_motor.as_mut(), id_motor, 19.0)?; + thread::sleep(Duration::from_millis(1100)); + let _reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor.as_mut(), id_motor)?; + println!("v_limit {:>5.1?}V", _reg); + let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_motor.as_mut(), id_motor, 0x00)?; + let reg = orbita2dof_foc::read_torque_enable (&io, serial_port_motor.as_mut(), id_motor)?; + if reg == 0x01 { + println!("Motor started... ({:#04x})", reg); + } else { + println!(":-("); + } + let reg = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor.as_mut(), id_motor)?; + println!("Position: {:?}", reg); + orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor.as_mut(), id_motor, 0.2)?; + + let mut display_counter = 0; + loop { + let t = now.elapsed().unwrap().as_secs_f32(); + let target = 1.0_f32 * 180.0_f32.to_radians() * (2.0 * PI * 0.1 * t).sin(); + +// orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor.as_mut(), id_motor, target)?; + + if display_counter == 0 { + //let encoder_pos = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor.as_mut(), id_motor)?; + let encoder_vel = orbita2dof_foc::read_motor_a_present_velocity(&io, serial_port_motor.as_mut(), id_motor)?; + let current_u = orbita2dof_foc::read_motor_a_current_phase_u(&io, serial_port_motor.as_mut(), id_motor)?; + let current_v = orbita2dof_foc::read_motor_a_current_phase_v(&io, serial_port_motor.as_mut(), id_motor)?; + let current_w = orbita2dof_foc::read_motor_a_current_phase_w(&io, serial_port_motor.as_mut(), id_motor)?; + let current_dc = orbita2dof_foc::read_motor_a_dc_current(&io, serial_port_motor.as_mut(), id_motor)?; + print!("[Enc_vel: {:6.2?}] - [Current.u.v.w: [{:6.3?} {:6.3?} {:6.3?}] .DC[{:6.3?}]]", encoder_vel, current_u, current_v, current_w, current_dc); + // let ring_pos = orbita2dof_foc::read_sensor_ring_present_position(&io, serial_port_motor.as_mut(), id_motor)?; +// let center_pos = orbita2dof_foc::read_sensor_center_present_position(&io, serial_port_motor.as_mut(), id_motor)?; +// print!("[Ring_s {:>5.3?} - Center_s: {:>5.3?}] -", ring_pos, center_pos); + } + + if display_counter == 0 { + println!(""); + } + display_counter += 1; + if display_counter > 10 { + display_counter = 0; + } + + thread::sleep(Duration::from_millis(10)); + } + +/* let mut serial_port_right_shoulder_motor_a = serialport::new("/dev/right_shoulder_A", 1_000_000) .timeout(Duration::from_millis(100)) .open()?; @@ -126,16 +183,16 @@ fn main() -> Result<(), Box> { let target_wrist = 60.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin(); -/* orbita_foc::write_goal_position( - &io, - serial_port_right_wrist.as_mut(), - id_right_wrist, - DiskValue { - top: target_wrist, - middle: target_wrist, - bottom: target_wrist, - }, - )?;*/ +// orbita_foc::write_goal_position( +// &io, +// serial_port_right_wrist.as_mut(), +// id_right_wrist, +// DiskValue { +// top: target_wrist, +// middle: target_wrist, +// bottom: target_wrist, +// }, +// )?; if display_counter == 0 { let shoulder_ring_pos = orbita2dof_foc::read_sensor_ring_present_position(&io, serial_port_right_shoulder_motor_a.as_mut(), id_right_shoulder_motor_a)?; @@ -156,7 +213,7 @@ fn main() -> Result<(), Box> { } thread::sleep(Duration::from_millis(10)); - } + }*/ } diff --git a/src/device/orbita2dof_foc.rs b/src/device/orbita2dof_foc.rs index a4c4a9c..4046802 100644 --- a/src/device/orbita2dof_foc.rs +++ b/src/device/orbita2dof_foc.rs @@ -60,10 +60,8 @@ reg_read_write!(temperature_limit, 54, f32); reg_read_write!(torque_enable, 58, u8); //reg_read_write!(goal_position, 59, DiskValue::); -reg_read_write!(roll_goal_position, 59, f32); -reg_read_write!(pitch_goal_position, 63, f32); - -//reg_read_only!(present_position_speed_load, 67, DiskPositionSpeedLoad); +reg_read_write!(ring_sensor_goal_position, 59, f32); +reg_read_write!(center_sensor_goal_position, 63, f32); //reg_read_only!(present_position, 67, DiskValue::); reg_read_only!(sensor_ring_present_position, 67, f32); reg_read_only!(sensor_center_present_position, 71, f32); @@ -95,6 +93,11 @@ reg_read_only!(imu_gyro_y, 135, f32); reg_read_only!(imu_gyro_z, 139, f32); reg_read_only!(imu_temperature, 143, f32); +reg_read_only!(motor_a_current_phase_u, 143, f32); +reg_read_only!(motor_a_current_phase_v, 147, f32); +reg_read_only!(motor_a_current_phase_w, 151, f32); +reg_read_only!(motor_a_dc_current, 155, f32); + impl PartialEq for DiskValue { fn eq(&self, other: &Self) -> bool { self.a == other.a && self.b == other.b From 4a75569f278b7694105a2aaa4eba1f0814b65a93 Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Mon, 19 Jun 2023 16:12:55 +0200 Subject: [PATCH 08/52] Delete foc_2dof.rs --- examples/foc_2dof.rs | 373 ------------------------------------------- 1 file changed, 373 deletions(-) delete mode 100644 examples/foc_2dof.rs diff --git a/examples/foc_2dof.rs b/examples/foc_2dof.rs deleted file mode 100644 index ec014b1..0000000 --- a/examples/foc_2dof.rs +++ /dev/null @@ -1,373 +0,0 @@ -use std::f32::consts::PI; -use std::time::SystemTime; -use std::{error::Error, thread, time::Duration}; - -use rustypot::device::orbita2dof_foc::{self}; -use rustypot::device::orbita_foc::{self, DiskValue}; -use rustypot::DynamixelSerialIO; - -const RIGHT_ARM_WRIST: u8 = 70; -const RIGHT_ARM_ELBOW_MOTOR_A: u8 = 71; -const RIGHT_ARM_ELBOW_MOTOR_B: u8 = 72; -const RIGHT_ARM_SHOULDER_MOTOR_A: u8 = 81; -const RIGHT_ARM_SHOULDER_MOTOR_B: u8 = 82; - -fn main() -> Result<(), Box> { - println!("Hi there! [foc_2dof.rs]"); -<<<<<<< HEAD - - let mut serial_port_motor = serialport::new("/dev/tool-XT90-A", 1_000_000) - .timeout(Duration::from_millis(100)) - .open()?; - - let now = SystemTime::now(); - let io = DynamixelSerialIO::v1(); - let id_motor = RIGHT_ARM_SHOULDER_MOTOR_B; - - let x = io.ping(serial_port_motor.as_mut(), id_motor); - println!("Ping ({:?}): {:?}", id_motor, x); - - let _reg = orbita2dof_foc::write_voltage_limit(&io, serial_port_motor.as_mut(), id_motor, 19.0)?; - thread::sleep(Duration::from_millis(1100)); - let _reg = orbita2dof_foc::read_voltage_limit(&io, serial_port_motor.as_mut(), id_motor)?; - println!("v_limit {:>5.1?}V", _reg); - let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_motor.as_mut(), id_motor, 0x00)?; - let reg = orbita2dof_foc::read_torque_enable (&io, serial_port_motor.as_mut(), id_motor)?; - if reg == 0x01 { - println!("Motor started... ({:#04x})", reg); - } else { - println!(":-("); - } - let reg = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor.as_mut(), id_motor)?; - println!("Position: {:?}", reg); - orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor.as_mut(), id_motor, 0.2)?; - - let mut display_counter = 0; - loop { - let t = now.elapsed().unwrap().as_secs_f32(); - let target = 1.0_f32 * 180.0_f32.to_radians() * (2.0 * PI * 0.1 * t).sin(); - -// orbita2dof_foc::write_motor_a_goal_position(&io, serial_port_motor.as_mut(), id_motor, target)?; - - if display_counter == 0 { - //let encoder_pos = orbita2dof_foc::read_motor_a_present_position(&io, serial_port_motor.as_mut(), id_motor)?; - let encoder_vel = orbita2dof_foc::read_motor_a_present_velocity(&io, serial_port_motor.as_mut(), id_motor)?; - let current_u = orbita2dof_foc::read_motor_a_current_phase_u(&io, serial_port_motor.as_mut(), id_motor)?; - let current_v = orbita2dof_foc::read_motor_a_current_phase_v(&io, serial_port_motor.as_mut(), id_motor)?; - let current_w = orbita2dof_foc::read_motor_a_current_phase_w(&io, serial_port_motor.as_mut(), id_motor)?; - let current_dc = orbita2dof_foc::read_motor_a_dc_current(&io, serial_port_motor.as_mut(), id_motor)?; - print!("[Enc_vel: {:6.2?}] - [Current.u.v.w: [{:6.3?} {:6.3?} {:6.3?}] .DC[{:6.3?}]]", encoder_vel, current_u, current_v, current_w, current_dc); - // let ring_pos = orbita2dof_foc::read_sensor_ring_present_position(&io, serial_port_motor.as_mut(), id_motor)?; -// let center_pos = orbita2dof_foc::read_sensor_center_present_position(&io, serial_port_motor.as_mut(), id_motor)?; -// print!("[Ring_s {:>5.3?} - Center_s: {:>5.3?}] -", ring_pos, center_pos); - } - - if display_counter == 0 { - println!(""); - } - display_counter += 1; - if display_counter > 10 { - display_counter = 0; - } - - thread::sleep(Duration::from_millis(10)); - } - -/* let mut serial_port_right_shoulder_motor_a = serialport::new("/dev/right_shoulder_A", 1_000_000) - .timeout(Duration::from_millis(100)) - .open()?; -======= ->>>>>>> 630e45b348819dbfcb3c14f7bcf3436173d37094 - - let mut serial_port_right_shoulder_motor_a = - serialport::new("/dev/right_shoulder_A", 1_000_000) - .timeout(Duration::from_millis(100)) - .open()?; - - let mut serial_port_right_shoulder_motor_b = - serialport::new("/dev/right_shoulder_B", 1_000_000) - .timeout(Duration::from_millis(100)) - .open()?; - - let mut serial_port_right_elbow_motor_a = serialport::new("/dev/right_elbow_A", 1_000_000) - .timeout(Duration::from_millis(100)) - .open()?; - - let mut serial_port_right_elbow_motor_b = serialport::new("/dev/right_elbow_B", 1_000_000) - .timeout(Duration::from_millis(100)) - .open()?; - - let mut serial_port_right_wrist = serialport::new("/dev/right_wrist", 1_000_000) - .timeout(Duration::from_millis(100)) - .open()?; - - let now = SystemTime::now(); - - let io = DynamixelSerialIO::v1(); - - let id_right_shoulder_motor_a = RIGHT_ARM_SHOULDER_MOTOR_A; - let id_right_shoulder_motor_b = RIGHT_ARM_SHOULDER_MOTOR_B; - let id_right_elbow_motor_a = RIGHT_ARM_ELBOW_MOTOR_A; - let id_right_elbow_motor_b = RIGHT_ARM_ELBOW_MOTOR_B; - let id_right_wrist = RIGHT_ARM_WRIST; - - // Ping - let x = io.ping( - serial_port_right_shoulder_motor_a.as_mut(), - id_right_shoulder_motor_a, - ); - println!("Ping (id_right_shoulder_motor_a): {:?}", x); - let x = io.ping( - serial_port_right_shoulder_motor_b.as_mut(), - id_right_shoulder_motor_b, - ); - println!("Ping (id_right_shoulder_motor_b): {:?}", x); - let x = io.ping( - serial_port_right_elbow_motor_a.as_mut(), - id_right_elbow_motor_a, - ); - println!("Ping (id_right_elbow_motor_a): {:?}", x); - let x = io.ping( - serial_port_right_elbow_motor_b.as_mut(), - id_right_elbow_motor_b, - ); - println!("Ping (id_right_elbow_motor_b): {:?}", x); - - let x = io.ping(serial_port_right_wrist.as_mut(), id_right_wrist); - println!("Ping (id_right_wrist): {:?}", x); - - // Set power - let _reg = orbita2dof_foc::write_voltage_limit( - &io, - serial_port_right_shoulder_motor_a.as_mut(), - id_right_shoulder_motor_a, - 10.0, - )?; - let _reg = orbita2dof_foc::write_voltage_limit( - &io, - serial_port_right_shoulder_motor_b.as_mut(), - id_right_shoulder_motor_b, - 10.0, - )?; - let _reg = orbita2dof_foc::write_voltage_limit( - &io, - serial_port_right_elbow_motor_a.as_mut(), - id_right_elbow_motor_a, - 10.0, - )?; - let _reg = orbita2dof_foc::write_voltage_limit( - &io, - serial_port_right_elbow_motor_b.as_mut(), - id_right_elbow_motor_b, - 10.0, - )?; - let _reg = orbita2dof_foc::read_voltage_limit( - &io, - serial_port_right_shoulder_motor_a.as_mut(), - id_right_shoulder_motor_a, - )?; - print!("v_limit {:>5.3?} -", _reg); - let _reg = orbita2dof_foc::read_voltage_limit( - &io, - serial_port_right_shoulder_motor_b.as_mut(), - id_right_shoulder_motor_b, - )?; - print!("v_limit {:>5.3?} -", _reg); - let _reg = orbita2dof_foc::read_voltage_limit( - &io, - serial_port_right_elbow_motor_a.as_mut(), - id_right_elbow_motor_a, - )?; - print!("v_limit {:>5.3?} -", _reg); - let _reg = orbita2dof_foc::read_voltage_limit( - &io, - serial_port_right_elbow_motor_b.as_mut(), - id_right_elbow_motor_b, - )?; - print!("v_limit {:>5.3?} -", _reg); - println!(""); - - // Torque enable/disable - let _reg = orbita2dof_foc::write_torque_enable( - &io, - serial_port_right_shoulder_motor_a.as_mut(), - id_right_shoulder_motor_a, - 0x01, - )?; - let reg = orbita2dof_foc::read_torque_enable( - &io, - serial_port_right_shoulder_motor_a.as_mut(), - id_right_shoulder_motor_a, - )?; - if reg == 0x01 { - println!("Shoulder - Motor A started... ({:#04x})", reg); - } else { - println!(":-("); - } - - let _reg = orbita2dof_foc::write_torque_enable( - &io, - serial_port_right_shoulder_motor_b.as_mut(), - id_right_shoulder_motor_b, - 0x01, - )?; - let reg = orbita2dof_foc::read_torque_enable( - &io, - serial_port_right_shoulder_motor_b.as_mut(), - id_right_shoulder_motor_b, - )?; - if reg == 0x01 { - println!("Shoulder - Motor B started... ({:#04x})", reg); - } else { - println!(":-("); - } - - let _reg = orbita2dof_foc::write_torque_enable( - &io, - serial_port_right_elbow_motor_a.as_mut(), - id_right_elbow_motor_a, - 0x01, - )?; - let reg = orbita2dof_foc::read_torque_enable( - &io, - serial_port_right_elbow_motor_a.as_mut(), - id_right_elbow_motor_a, - )?; - if reg == 0x01 { - println!("Elbow - Motor A started... ({:#04x})", reg); - } else { - println!(":-("); - } - - let _reg = orbita2dof_foc::write_torque_enable( - &io, - serial_port_right_elbow_motor_b.as_mut(), - id_right_elbow_motor_b, - 0x01, - )?; - let reg = orbita2dof_foc::read_torque_enable( - &io, - serial_port_right_elbow_motor_b.as_mut(), - id_right_elbow_motor_b, - )?; - if reg == 0x01 { - println!("Elbow - Motor B started... ({:#04x})", reg); - } else { - println!(":-("); - } - - orbita_foc::write_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist, 0x01)?; - - // let _reg = orbita2dof_foc::write_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist, 0x01)?; - // let reg = orbita2dof_foc::read_torque_enable(&io, serial_port_right_wrist.as_mut(), id_right_wrist)?; - // if reg == 0x01 { - // println!("Wrist started... ({:#04x})", reg); - // } else { - // println!(":-("); - // } - - let mut display_counter = 0; - loop { - let t = now.elapsed().unwrap().as_secs_f32(); - let target = 10.0_f32 * 180.0_f32.to_radians() * (2.0 * PI * 0.1 * t).sin(); - - orbita2dof_foc::write_motor_a_goal_position( - &io, - serial_port_right_shoulder_motor_a.as_mut(), - id_right_shoulder_motor_a, - target, - )?; - orbita2dof_foc::write_motor_a_goal_position( - &io, - serial_port_right_shoulder_motor_b.as_mut(), - id_right_shoulder_motor_b, - 1.0_f32 * target, - )?; - orbita2dof_foc::write_motor_a_goal_position( - &io, - serial_port_right_elbow_motor_a.as_mut(), - id_right_elbow_motor_a, - -1.0_f32 * target, - )?; - orbita2dof_foc::write_motor_a_goal_position( - &io, - serial_port_right_elbow_motor_b.as_mut(), - id_right_elbow_motor_b, - 1.0_f32 * target, - )?; - -<<<<<<< HEAD -// orbita_foc::write_goal_position( -// &io, -// serial_port_right_wrist.as_mut(), -// id_right_wrist, -// DiskValue { -// top: target_wrist, -// middle: target_wrist, -// bottom: target_wrist, -// }, -// )?; - -======= - let target_wrist = 60.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin(); - let disks = orbita_foc::read_present_position( - &io, - serial_port_right_wrist.as_mut(), - id_right_wrist, - )?; - - orbita_foc::write_goal_position( - &io, - serial_port_right_wrist.as_mut(), - id_right_wrist, - DiskValue { - top: disks.top + target_wrist, - middle: disks.middle + target_wrist, - bottom: disks.bottom + target_wrist, - }, - )?; - ->>>>>>> 630e45b348819dbfcb3c14f7bcf3436173d37094 - if display_counter == 0 { - let shoulder_ring_pos = orbita2dof_foc::read_sensor_ring_present_position( - &io, - serial_port_right_shoulder_motor_a.as_mut(), - id_right_shoulder_motor_a, - )?; - let shoulder_center_pos = orbita2dof_foc::read_sensor_center_present_position( - &io, - serial_port_right_shoulder_motor_b.as_mut(), - id_right_shoulder_motor_b, - )?; - let elbow_ring_pos = orbita2dof_foc::read_sensor_ring_present_position( - &io, - serial_port_right_elbow_motor_a.as_mut(), - id_right_elbow_motor_a, - )?; - let elbow_center_pos = orbita2dof_foc::read_sensor_center_present_position( - &io, - serial_port_right_elbow_motor_b.as_mut(), - id_right_elbow_motor_b, - )?; - - print!( - "[Shoulder Ring_s {:>5.3?} - Center_s: {:>5.3?}] -", - shoulder_ring_pos, shoulder_center_pos - ); - print!( - "[Elbow Ring_s {:>5.3?} - Center_s: {:>5.3?}] -", - elbow_ring_pos, elbow_center_pos - ); - } - - if display_counter == 0 { - println!(""); - } - display_counter += 1; - if display_counter > 10 { - display_counter = 0; - } - - thread::sleep(Duration::from_millis(10)); - }*/ -} From dd4b667a99d5319a811cb57baf8df695bebd2fbc Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Mon, 19 Jun 2023 16:13:43 +0200 Subject: [PATCH 09/52] Update foc.rs --- examples/foc.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/foc.rs b/examples/foc.rs index 757851b..789b6fd 100644 --- a/examples/foc.rs +++ b/examples/foc.rs @@ -13,7 +13,7 @@ fn main() -> Result<(), Box> { let io = DynamixelSerialIO::v1(); let id = 70; - orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 0); + orbita_foc::write_torque_enable(&io, serial_port.as_mut(), id, 0)?; thread::sleep(Duration::from_millis(100)); From d1112066e351055b458d7ff4eb5c2c1c20e7396f Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Mon, 19 Jun 2023 16:14:24 +0200 Subject: [PATCH 10/52] Update mod.rs --- src/device/mod.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/device/mod.rs b/src/device/mod.rs index bb6b4b6..cc2f346 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -89,6 +89,6 @@ macro_rules! reg_read_write { pub mod l0_force_fan; pub mod mx; -pub mod orbita_foc; pub mod orbita2dof_foc; +pub mod orbita_foc; pub mod xl320; From 3abde569991a85d66094faec4c4ef81134c9cdcd Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Mon, 19 Jun 2023 16:22:00 +0200 Subject: [PATCH 11/52] Update orbita2dof_foc.rs --- src/device/orbita2dof_foc.rs | 63 ------------------------------------ 1 file changed, 63 deletions(-) diff --git a/src/device/orbita2dof_foc.rs b/src/device/orbita2dof_foc.rs index 4046802..db4006a 100644 --- a/src/device/orbita2dof_foc.rs +++ b/src/device/orbita2dof_foc.rs @@ -2,13 +2,6 @@ use crate::device::*; -/// Wrapper for a value per disk (top, middle, bottom) -#[derive(Clone, Copy, Debug)] -pub struct DiskValue { - pub a: T, - pub b: T, -} - /// Wrapper for a 3D vector (x, y, z) #[derive(Clone, Copy, Debug)] pub struct Vec3d { @@ -17,13 +10,6 @@ pub struct Vec3d { pub z: T, } -/// Wrapper for a Position/Speed/Load value for each disk -#[derive(Clone, Copy, Debug)] -pub struct DiskPositionSpeedLoad { - pub position: DiskValue, - pub speed: DiskValue, - pub load: DiskValue, -} /// Wrapper for PID gains. #[derive(Clone, Copy, Debug, PartialEq)] pub struct Pid { @@ -59,27 +45,20 @@ reg_read_write!(temperature_limit, 54, f32); reg_read_write!(torque_enable, 58, u8); -//reg_read_write!(goal_position, 59, DiskValue::); reg_read_write!(ring_sensor_goal_position, 59, f32); reg_read_write!(center_sensor_goal_position, 63, f32); -//reg_read_only!(present_position, 67, DiskValue::); reg_read_only!(sensor_ring_present_position, 67, f32); reg_read_only!(sensor_center_present_position, 71, f32); -//reg_read_only!(motors_goal_position, 75, DiskValue::); reg_read_write!(motor_a_goal_position, 75, f32); reg_read_write!(motor_b_goal_position, 79, f32); -//reg_read_only!(motors_present_position, 83, DiskValue::); reg_read_only!(motor_a_present_position, 83, f32); reg_read_only!(motor_b_present_position, 87, f32); -//reg_read_only!(motors_present_velocity, 91, DiskValue::); reg_read_only!(motor_a_present_velocity, 91, f32); reg_read_only!(motor_b_present_velocity, 95, f32); -//reg_read_only!(motors_present_load, 99, DiskValue::); reg_read_only!(motor_a_present_load, 99, f32); reg_read_only!(motor_b_present_load, 103, f32); -//reg_read_only!(present_temperature, 107, DiskValue::); reg_read_only!(motor_a_present_temperature, 107, f32); reg_read_only!(motor_b_present_temperature, 111, f32); @@ -98,29 +77,6 @@ reg_read_only!(motor_a_current_phase_v, 147, f32); reg_read_only!(motor_a_current_phase_w, 151, f32); reg_read_only!(motor_a_dc_current, 155, f32); -impl PartialEq for DiskValue { - fn eq(&self, other: &Self) -> bool { - self.a == other.a && self.b == other.b - } -} - -impl DiskValue { - pub fn from_le_bytes(bytes: [u8; 8]) -> Self { - DiskValue { - a: f32::from_le_bytes(bytes[0..4].try_into().unwrap()), - b: f32::from_le_bytes(bytes[4..8].try_into().unwrap()), - } - } - pub fn to_le_bytes(&self) -> [u8; 8] { - let mut bytes = Vec::new(); - - bytes.extend_from_slice(&self.a.to_le_bytes()); - bytes.extend_from_slice(&self.b.to_le_bytes()); - - bytes.try_into().unwrap() - } -} - impl PartialEq for Vec3d { fn eq(&self, other: &Self) -> bool { self.x == other.x && self.y == other.y && self.z == other.z @@ -164,22 +120,3 @@ impl Pid { bytes.try_into().unwrap() } } -/* -impl DiskPositionSpeedLoad { - pub fn from_le_bytes(bytes: [u8; 36]) -> Self { - DiskPositionSpeedLoad { - position: DiskValue::from_le_bytes(bytes[0..12].try_into().unwrap()), - speed: DiskValue::from_le_bytes(bytes[12..24].try_into().unwrap()), - load: DiskValue::from_le_bytes(bytes[24..36].try_into().unwrap()), - } - } - pub fn to_le_bytes(&self) -> [u8; 36] { - let mut bytes = Vec::new(); - - bytes.extend_from_slice(&self.position.to_le_bytes()); - bytes.extend_from_slice(&self.speed.to_le_bytes()); - bytes.extend_from_slice(&self.load.to_le_bytes()); - - bytes.try_into().unwrap() - } -}*/ From f7c831916618388354745ab5049a05bc8c47545a Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Mon, 19 Jun 2023 16:23:57 +0200 Subject: [PATCH 12/52] Prepare release 0.4.0 --- Cargo.toml | 2 +- Changelog.md | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 41c75cd..6d12ec2 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rustypot" -version = "0.3.1" +version = "0.4.0" edition = "2021" license = "Apache-2.0" authors = ["Pollen Robotics"] diff --git a/Changelog.md b/Changelog.md index 4fa61b2..6486da7 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,3 +1,7 @@ +## Version 0.4.0 + +- Add support for orbita-2dof-foc device. + ### Version 0.3.1 - Patch torque limit conversion. From 531dda551cb3ba78dd11826ecb95826dd04d5a2e Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Wed, 5 Jul 2023 14:27:24 +0200 Subject: [PATCH 13/52] add intensity_limit and current measure registers --- src/device/orbita2dof_foc.rs | 45 ++++++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/src/device/orbita2dof_foc.rs b/src/device/orbita2dof_foc.rs index db4006a..c1ae2f5 100644 --- a/src/device/orbita2dof_foc.rs +++ b/src/device/orbita2dof_foc.rs @@ -2,6 +2,13 @@ use crate::device::*; +/// Wrapper for a value per motor (A and B) +#[derive(Clone, Copy, Debug)] +pub struct MotorValue { + pub a: T, + pub b: T, +} + /// Wrapper for a 3D vector (x, y, z) #[derive(Clone, Copy, Debug)] pub struct Vec3d { @@ -10,6 +17,13 @@ pub struct Vec3d { pub z: T, } +/// Wrapper for a Position/Speed/Load value for each motor +#[derive(Clone, Copy, Debug)] +pub struct MotorPositionSpeedLoad { + pub position: MotorValue, + pub speed: MotorValue, + pub load: MotorValue, +} /// Wrapper for PID gains. #[derive(Clone, Copy, Debug, PartialEq)] pub struct Pid { @@ -47,15 +61,19 @@ reg_read_write!(torque_enable, 58, u8); reg_read_write!(ring_sensor_goal_position, 59, f32); reg_read_write!(center_sensor_goal_position, 63, f32); + reg_read_only!(sensor_ring_present_position, 67, f32); reg_read_only!(sensor_center_present_position, 71, f32); reg_read_write!(motor_a_goal_position, 75, f32); reg_read_write!(motor_b_goal_position, 79, f32); + reg_read_only!(motor_a_present_position, 83, f32); reg_read_only!(motor_b_present_position, 87, f32); + reg_read_only!(motor_a_present_velocity, 91, f32); reg_read_only!(motor_b_present_velocity, 95, f32); + reg_read_only!(motor_a_present_load, 99, f32); reg_read_only!(motor_b_present_load, 103, f32); @@ -77,6 +95,33 @@ reg_read_only!(motor_a_current_phase_v, 147, f32); reg_read_only!(motor_a_current_phase_w, 151, f32); reg_read_only!(motor_a_dc_current, 155, f32); +reg_read_write!(debug_float_1, 159, f32); +reg_read_write!(debug_float_2, 163, f32); +reg_read_write!(debug_float_3, 167, f32); + +impl PartialEq for MotorValue { + fn eq(&self, other: &Self) -> bool { + self.a == other.a && self.b == other.b + } +} + +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 8]) -> Self { + MotorValue { + a: f32::from_le_bytes(bytes[0..4].try_into().unwrap()), + b: f32::from_le_bytes(bytes[4..8].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 8] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.a.to_le_bytes()); + bytes.extend_from_slice(&self.b.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + impl PartialEq for Vec3d { fn eq(&self, other: &Self) -> bool { self.x == other.x && self.y == other.y && self.z == other.z From 2fb24befa793c2a0b9e48ce6b801982854b453cc Mon Sep 17 00:00:00 2001 From: SteveNguyen Date: Fri, 7 Jul 2023 09:35:21 +0000 Subject: [PATCH 14/52] Update Cargo.toml --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 6d12ec2..ea7e7ec 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rustypot" -version = "0.4.0" +version = "0.4.1" edition = "2021" license = "Apache-2.0" authors = ["Pollen Robotics"] From 33ab7c842e32dc722362dbc14fd9f437d27ba417 Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Tue, 22 Aug 2023 16:06:59 +0200 Subject: [PATCH 15/52] Impl Debug trait. --- Cargo.toml | 2 +- src/lib.rs | 2 ++ src/protocol/v1.rs | 4 ++++ src/protocol/v2.rs | 4 ++++ 4 files changed, 11 insertions(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 6d12ec2..ea7e7ec 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rustypot" -version = "0.4.0" +version = "0.4.1" edition = "2021" license = "Apache-2.0" authors = ["Pollen Robotics"] diff --git a/src/lib.rs b/src/lib.rs index c853759..73309aa 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -38,11 +38,13 @@ pub mod device; type Result = std::result::Result>; +#[derive(Debug)] enum Protocols { V1(V1), V2(V2), } +#[derive(Debug)] /// Raw dynamixel communication messages controller (protocol v1 or v2) pub struct DynamixelSerialIO { protocol: Protocols, diff --git a/src/protocol/v1.rs b/src/protocol/v1.rs index 440d66a..5154e83 100644 --- a/src/protocol/v1.rs +++ b/src/protocol/v1.rs @@ -8,6 +8,7 @@ use super::{CommunicationErrorKind, Packet}; const BROADCAST_ID: u8 = 254; const BROADCAST_RESPONSE_ID: u8 = 253; +#[derive(Debug)] pub struct PacketV1; impl Packet for PacketV1 { const HEADER_SIZE: usize = 4; @@ -94,6 +95,7 @@ impl Packet for PacketV1 { } } +#[derive(Debug)] struct InstructionPacketV1 { id: u8, instruction: InstructionKindV1, @@ -126,6 +128,7 @@ impl InstructionPacket for InstructionPacketV1 { } } +#[derive(Debug)] struct StatusPacketV1 { id: u8, #[allow(dead_code)] @@ -235,6 +238,7 @@ impl InstructionKindV1 { } } +#[derive(Debug)] pub struct V1; impl Protocol for V1 { fn new() -> Self { diff --git a/src/protocol/v2.rs b/src/protocol/v2.rs index e1b706d..1a0aabe 100644 --- a/src/protocol/v2.rs +++ b/src/protocol/v2.rs @@ -5,6 +5,7 @@ use crate::{ use super::{CommunicationErrorKind, Protocol}; +#[derive(Debug)] pub struct V2; impl Protocol for V2 { fn new() -> Self @@ -35,6 +36,7 @@ impl Protocol for V2 { } } +#[derive(Debug)] pub struct PacketV2; impl Packet for PacketV2 { const HEADER_SIZE: usize = 7; @@ -132,6 +134,7 @@ impl Packet for PacketV2 { } } +#[derive(Debug)] struct InstructionPacketV2 { id: u8, instruction: InstructionKindV2, @@ -169,6 +172,7 @@ impl InstructionPacket for InstructionPacketV2 { } } +#[derive(Debug)] struct StatusPacketV2 { id: u8, errors: Vec, From 75481a4c74ab975357736b243fd415cb8e31a5cf Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Tue, 22 Aug 2023 16:09:59 +0200 Subject: [PATCH 16/52] Apply clippy rules. --- src/device/mx.rs | 2 +- src/protocol/v1.rs | 7 +++---- src/protocol/v2.rs | 3 +-- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/device/mx.rs b/src/device/mx.rs index e254529..d664ad3 100644 --- a/src/device/mx.rs +++ b/src/device/mx.rs @@ -135,7 +135,7 @@ pub mod conv { /// /// Works for torque_limit for instance pub fn torque_to_dxl_abs_load(torque: f64) -> u16 { - assert!(torque >= 0.0 && torque <= 100.0); + assert!((0.0..=100.0).contains(&torque)); (torque * 1023.0 / 100.0) as u16 } diff --git a/src/protocol/v1.rs b/src/protocol/v1.rs index 440d66a..162392d 100644 --- a/src/protocol/v1.rs +++ b/src/protocol/v1.rs @@ -195,7 +195,6 @@ pub enum DynamixelErrorV1 { impl DynamixelErrorV1 { fn from_byte(error: u8) -> Vec { (0..7) - .into_iter() .filter(|i| error & (1 << i) != 0) .map(|i| DynamixelErrorV1::from_bit(i).unwrap()) .collect() @@ -302,11 +301,11 @@ mod tests { #[test] fn create_write_packet() { - let p = PacketV1::write_packet(10, 24, &vec![1]); + let p = PacketV1::write_packet(10, 24, &[1]); let bytes = p.to_bytes(); assert_eq!(bytes, [255, 255, 10, 4, 3, 24, 1, 213]); - let p = PacketV1::write_packet(0xFE, 0x03, &vec![1]); + let p = PacketV1::write_packet(0xFE, 0x03, &[1]); let bytes = p.to_bytes(); assert_eq!(bytes, [0xFF, 0xFF, 0xFE, 0x04, 0x03, 0x03, 0x01, 0xF6]); } @@ -323,7 +322,7 @@ mod tests { #[test] fn create_sync_write_packet() { - let p = PacketV1::sync_write_packet(&[11, 12], 30, &vec![vec![0x0, 0x0], vec![0xA, 0x14]]); + let p = PacketV1::sync_write_packet(&[11, 12], 30, &[vec![0x0, 0x0], vec![0xA, 0x14]]); let bytes = p.to_bytes(); assert_eq!( bytes, diff --git a/src/protocol/v2.rs b/src/protocol/v2.rs index e1b706d..823a663 100644 --- a/src/protocol/v2.rs +++ b/src/protocol/v2.rs @@ -267,7 +267,6 @@ pub enum DynamixelErrorV2 { impl DynamixelErrorV2 { fn from_byte(error: u8) -> Vec { (1..7) - .into_iter() .filter(|i| error & (1 << i) != 0) .map(|i| DynamixelErrorV2::from_bit(i).unwrap()) .collect() @@ -385,7 +384,7 @@ mod tests { let p = PacketV2::sync_write_packet( &[1, 2], 116, - &vec![ + &[ 150_u32.to_le_bytes().to_vec(), 170_u32.to_le_bytes().to_vec(), ], From e1e7820f3e60a47d41cdbb676e4477b66824f0d0 Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Tue, 22 Aug 2023 16:11:20 +0200 Subject: [PATCH 17/52] Update Cargo.toml --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 6d12ec2..2636469 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rustypot" -version = "0.4.0" +version = "0.4.2" edition = "2021" license = "Apache-2.0" authors = ["Pollen Robotics"] From fac69926c6d935388d49cc456b4aad2d0e553ee5 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Mon, 30 Oct 2023 14:59:21 +0100 Subject: [PATCH 18/52] test poulpe --- Cargo.toml | 1 + examples/foc.rs | 50 ++++++++++++++++----- examples/poulpe.rs | 96 ++++++++++++++++++++++++++++++++++++++++ src/device/mod.rs | 1 + src/device/orbita_foc.rs | 8 +++- src/protocol/v1.rs | 5 +++ 6 files changed, 149 insertions(+), 12 deletions(-) create mode 100644 examples/poulpe.rs diff --git a/Cargo.toml b/Cargo.toml index 2636469..1177d6b 100644 --- a/Cargo.toml +++ b/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" diff --git a/examples/foc.rs b/examples/foc.rs index 789b6fd..0dd471a 100644 --- a/examples/foc.rs +++ b/examples/foc.rs @@ -13,36 +13,64 @@ fn main() -> Result<(), Box> { 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(()) } diff --git a/examples/poulpe.rs b/examples/poulpe.rs new file mode 100644 index 0000000..8877ecf --- /dev/null +++ b/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> { + 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(()) +} diff --git a/src/device/mod.rs b/src/device/mod.rs index cc2f346..b0c75d6 100644 --- a/src/device/mod.rs +++ b/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; diff --git a/src/device/orbita_foc.rs b/src/device/orbita_foc.rs index ab6aec3..62d790a 100644 --- a/src/device/orbita_foc.rs +++ b/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::); 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::); // reg_read_only!(top_present_speed, 83, f32); // reg_read_only!(middle_present_speed, 87, f32); diff --git a/src/protocol/v1.rs b/src/protocol/v1.rs index 1b73421..ff740b1 100644 --- a/src/protocol/v1.rs +++ b/src/protocol/v1.rs @@ -148,6 +148,11 @@ impl StatusPacket 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)); } From 5f19dc687d0d172847787c22f1953d4e962b1f02 Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Wed, 15 Nov 2023 11:49:18 +0100 Subject: [PATCH 19/52] Add basic reg for orbita2d/3d poulpe. --- src/device/mod.rs | 2 ++ src/device/orbita2d_poulpe.rs | 44 +++++++++++++++++++++++++++++++ src/device/orbita3d_poulpe.rs | 49 +++++++++++++++++++++++++++++++++++ 3 files changed, 95 insertions(+) create mode 100644 src/device/orbita2d_poulpe.rs create mode 100644 src/device/orbita3d_poulpe.rs diff --git a/src/device/mod.rs b/src/device/mod.rs index cc2f346..e369b75 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -89,6 +89,8 @@ macro_rules! reg_read_write { pub mod l0_force_fan; pub mod mx; +pub mod orbita2d_poulpe; pub mod orbita2dof_foc; +pub mod orbita3d_poulpe; pub mod orbita_foc; pub mod xl320; diff --git a/src/device/orbita2d_poulpe.rs b/src/device/orbita2d_poulpe.rs new file mode 100644 index 0000000..8649123 --- /dev/null +++ b/src/device/orbita2d_poulpe.rs @@ -0,0 +1,44 @@ +use crate::device::*; + +pub struct ValuePerMotor { + pub motor_a: T, + pub motor_b: T, +} + +reg_read_write!(torque_enable, 40, ValuePerMotor::); +reg_read_write!(present_position, 50, ValuePerMotor::); +reg_read_write!(goal_position, 60, ValuePerMotor::); + +impl ValuePerMotor { + pub fn from_le_bytes(bytes: [u8; 2]) -> Self { + ValuePerMotor { + motor_a: bytes[0], + motor_b: bytes[1], + } + } + pub fn to_le_bytes(&self) -> [u8; 3] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.motor_a.to_le_bytes()); + bytes.extend_from_slice(&self.motor_b.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + +impl ValuePerMotor { + pub fn from_le_bytes(bytes: [u8; 8]) -> Self { + ValuePerMotor { + motor_a: f32::from_le_bytes(bytes[0..4].try_into().unwrap()), + motor_b: f32::from_le_bytes(bytes[4..8].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 8] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.motor_a.to_le_bytes()); + bytes.extend_from_slice(&self.motor_b.to_le_bytes()); + + bytes.try_into().unwrap() + } +} diff --git a/src/device/orbita3d_poulpe.rs b/src/device/orbita3d_poulpe.rs new file mode 100644 index 0000000..6c9a2d7 --- /dev/null +++ b/src/device/orbita3d_poulpe.rs @@ -0,0 +1,49 @@ +use crate::device::*; + +pub struct ValuePerMotor { + pub top: T, + pub middle: T, + pub bottom: T, +} + +reg_read_write!(torque_enable, 40, ValuePerMotor::); +reg_read_write!(present_position, 50, ValuePerMotor::); +reg_read_write!(goal_position, 60, ValuePerMotor::); + +impl ValuePerMotor { + pub fn from_le_bytes(bytes: [u8; 3]) -> Self { + ValuePerMotor { + top: bytes[0], + middle: bytes[1], + bottom: bytes[2], + } + } + pub fn to_le_bytes(&self) -> [u8; 3] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.top.to_le_bytes()); + bytes.extend_from_slice(&self.middle.to_le_bytes()); + bytes.extend_from_slice(&self.bottom.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + +impl ValuePerMotor { + pub fn from_le_bytes(bytes: [u8; 12]) -> Self { + ValuePerMotor { + top: f32::from_le_bytes(bytes[0..4].try_into().unwrap()), + middle: f32::from_le_bytes(bytes[4..8].try_into().unwrap()), + bottom: f32::from_le_bytes(bytes[8..12].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 12] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.top.to_le_bytes()); + bytes.extend_from_slice(&self.middle.to_le_bytes()); + bytes.extend_from_slice(&self.bottom.to_le_bytes()); + + bytes.try_into().unwrap() + } +} From 50af17e36bd2f86ce61bba47fda1479323b61ba7 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 2 Jan 2024 18:35:19 +0100 Subject: [PATCH 20/52] add feeback mechanism --- examples/poulpe3d.rs | 107 +++++++++++++++++++++++ src/device/mod.rs | 51 ++++++++++- src/device/poulpe3d.rs | 190 +++++++++++++++++++++++++++++++++++++++++ src/lib.rs | 17 ++++ src/protocol/mod.rs | 10 +++ 5 files changed, 374 insertions(+), 1 deletion(-) create mode 100644 examples/poulpe3d.rs create mode 100644 src/device/poulpe3d.rs diff --git a/examples/poulpe3d.rs b/examples/poulpe3d.rs new file mode 100644 index 0000000..f346c1b --- /dev/null +++ b/examples/poulpe3d.rs @@ -0,0 +1,107 @@ +use std::f32::consts::PI; +use std::time::SystemTime; +use std::{error::Error, thread, time::Duration, time::Instant}; + +use rustypot::device::poulpe3d::{self, MotorValue}; +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> { + 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 _ = poulpe3d::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:true, middle:true, bottom:true})?; + thread::sleep(Duration::from_millis(1000)); + let torque = poulpe3d::read_torque_enable(&io, serial_port.as_mut(), id)?; + println!("torque: {:?}", torque); + thread::sleep(Duration::from_millis(1000)); + + let mut t = now.elapsed().unwrap().as_secs_f32(); + loop { + let t0 = Instant::now(); + + 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 feedback = poulpe3d::write_target_position(&io, serial_port.as_mut(), id, MotorValue::{top:target, middle:target, bottom:target})?; + + + println!("target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); + + println!("ELAPSED: {:?}",t0.elapsed().as_micros()); + // thread::sleep(Duration::from_micros(1000-t0.elapsed().as_micros() as u64)); + 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"); + // } + let _ = poulpe3d::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:false, middle:false, bottom:false})?; + + + thread::sleep(Duration::from_millis(2000)); + + Ok(()) +} diff --git a/src/device/mod.rs b/src/device/mod.rs index b0c75d6..e2ad681 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -78,6 +78,45 @@ macro_rules! reg_write_only { }; } +/// Generates write and sync_write functions with feedback for given register +#[macro_export] +macro_rules! reg_write_only_fb { + ($name:ident, $addr:expr, $reg_type:ty, $fb_type: ty) => { + paste! { + #[doc = concat!("Write register with fb *", stringify!($name), "* (addr: ", stringify!($addr), ", type: ", stringify!($reg_type), ")")] + pub fn []( + io: &DynamixelSerialIO, + serial_port: &mut dyn serialport::SerialPort, + id: u8, + val: $reg_type, + ) -> Result<$fb_type> { + let fb=io.write_fb(serial_port, id, $addr, &val.to_le_bytes())?; + let fb = $fb_type::from_le_bytes(fb.try_into().unwrap()); + Ok(fb) + } + + #[doc = concat!("Sync write register *", stringify!($name), "* (addr: ", stringify!($addr), ", type: ", stringify!($reg_type), ")")] + pub fn []( + io: &DynamixelSerialIO, + serial_port: &mut dyn serialport::SerialPort, + ids: &[u8], + values: &[$reg_type], + ) -> Result<()> { + io.sync_write( + serial_port, + ids, + $addr, + &values + .iter() + .map(|v| v.to_le_bytes().to_vec()) + .collect::>>(), + ) + } + } + }; +} + + /// Generates read, sync_read, write and sync_write functions for given register #[macro_export] macro_rules! reg_read_write { @@ -87,9 +126,19 @@ macro_rules! reg_read_write { }; } +#[macro_export] +macro_rules! reg_read_write_fb { + ($name:ident, $addr:expr, $reg_type:ty, $fb_type: ty) => { + reg_read_only!($name, $addr, $reg_type); + reg_write_only_fb!($name, $addr, $reg_type, $fb_type); + }; +} + + pub mod l0_force_fan; pub mod mx; pub mod orbita2dof_foc; pub mod orbita_foc; -pub mod poulpe; + pub mod xl320; +pub mod poulpe3d; diff --git a/src/device/poulpe3d.rs b/src/device/poulpe3d.rs new file mode 100644 index 0000000..6215458 --- /dev/null +++ b/src/device/poulpe3d.rs @@ -0,0 +1,190 @@ +//! Orbita 2DoF Serial SimpleFOC register (protocol v1) + +use crate::device::*; + +/// Wrapper for a value per motor (A and B) +#[derive(Clone, Copy, Debug)] +pub struct MotorValue { + pub top: T, + pub middle: T, + pub bottom: T, +} + +/// Wrapper for a 3D vector (x, y, z) +#[derive(Clone, Copy, Debug)] +pub struct Vec3d { + pub x: T, + pub y: T, + pub z: T, +} + +/// Wrapper for a Position/Speed/Load value for each motor +#[derive(Clone, Copy, Debug)] +pub struct MotorPositionSpeedLoad { + pub position: MotorValue, + pub speed: MotorValue, + pub load: MotorValue, +} +/// Wrapper for PID gains. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Pid { + pub p: i16, + pub i: i16, +} + +reg_read_only!(model_number, 0, u16); +reg_read_only!(firmware_version, 6, u8); +reg_read_write!(id, 7, u8); + +reg_read_write!(velocity_limit, 10, MotorValue::); +reg_read_write!(torque_flux_limit, 14, MotorValue::); +reg_read_write!(uq_ud_limit, 18, MotorValue::); + +reg_read_write!(flux_pid, 20, MotorValue::); +reg_read_write!(torque_pid, 24, MotorValue::); +reg_read_write!(velocity_pid, 28, MotorValue::); +reg_read_write!(position_pid, 32, MotorValue::); + +reg_read_write!(torque_enable, 40, MotorValue::); + +reg_read_only!(current_position, 50, MotorValue::); +reg_read_only!(current_velocity, 51, MotorValue::); +reg_read_only!(current_torque, 52, MotorValue::); + +reg_read_write_fb!(target_position, 60, MotorValue::,MotorPositionSpeedLoad); + +reg_read_only!(axis_sensor, 90, MotorValue::); + +reg_read_only!(full_state, 100, MotorPositionSpeedLoad); + + + +impl MotorPositionSpeedLoad { + pub fn from_le_bytes(bytes: [u8; 36]) -> Self { + MotorPositionSpeedLoad { + position: MotorValue::::from_le_bytes(bytes[0..12].try_into().unwrap()), + speed: MotorValue::::from_le_bytes(bytes[12..24].try_into().unwrap()), + load: MotorValue::::from_le_bytes(bytes[24..36].try_into().unwrap()), + } + } + // pub fn to_le_bytes(&self) -> [u8; 36] { + // let mut bytes = Vec::new(); + + // bytes.extend_from_slice(&self.position.to_le_bytes()); + // bytes.extend_from_slice(&self.speed.to_le_bytes()); + // bytes.extend_from_slice(&self.load.to_le_bytes()); + + // bytes.try_into().unwrap() + // } +} + + + + +impl PartialEq for MotorValue { + fn eq(&self, other: &Self) -> bool { + self.top == other.top && self.middle == other.middle && self.bottom == other.bottom + } +} + +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 12]) -> Self { + MotorValue { + top: f32::from_le_bytes(bytes[0..4].try_into().unwrap()), + middle: f32::from_le_bytes(bytes[4..8].try_into().unwrap()), + bottom: f32::from_le_bytes(bytes[8..12].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 12] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.top.to_le_bytes()); + bytes.extend_from_slice(&self.middle.to_le_bytes()); + bytes.extend_from_slice(&self.bottom.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + + +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 3]) -> Self { + MotorValue { + top: bytes[0] !=0, + middle: bytes[1] !=0, + bottom: bytes[2] !=0, + } + } + pub fn to_le_bytes(&self) -> [u8; 3] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&[self.top as u8]); + bytes.extend_from_slice(&[self.middle as u8]); + bytes.extend_from_slice(&[self.bottom as u8]); + + bytes.try_into().unwrap() + } +} + + +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 12]) -> Self { + MotorValue { + top: Pid::from_le_bytes(bytes[0..4].try_into().unwrap()), + middle: Pid::from_le_bytes(bytes[4..8].try_into().unwrap()), + bottom: Pid::from_le_bytes(bytes[8..12].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 12] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.top.to_le_bytes()); + bytes.extend_from_slice(&self.middle.to_le_bytes()); + bytes.extend_from_slice(&self.bottom.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + + +impl PartialEq for Vec3d { + fn eq(&self, other: &Self) -> bool { + self.x == other.x && self.y == other.y && self.z == other.z + } +} + +impl Vec3d { + pub fn from_le_bytes(bytes: [u8; 12]) -> Self { + Vec3d { + x: f32::from_le_bytes(bytes[0..4].try_into().unwrap()), + y: f32::from_le_bytes(bytes[4..8].try_into().unwrap()), + z: f32::from_le_bytes(bytes[8..12].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 12] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.x.to_le_bytes()); + bytes.extend_from_slice(&self.y.to_le_bytes()); + bytes.extend_from_slice(&self.z.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + +impl Pid { + pub fn from_le_bytes(bytes: [u8; 4]) -> Self { + Pid { + p: i16::from_le_bytes(bytes[0..2].try_into().unwrap()), + i: i16::from_le_bytes(bytes[2..4].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 4] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.p.to_le_bytes()); + bytes.extend_from_slice(&self.i.to_le_bytes()); + + bytes.try_into().unwrap() + } +} diff --git a/src/lib.rs b/src/lib.rs index 73309aa..bb98188 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -220,6 +220,23 @@ impl DynamixelSerialIO { } } + + + pub fn write_fb( + &self, + serial_port: &mut dyn serialport::SerialPort, + id: u8, + addr: u8, + data: &[u8], + ) -> Result> { + match &self.protocol { + Protocols::V1(p) => p.write_fb(serial_port, id, addr, data), + Protocols::V2(p) => Err(Box::new(CommunicationErrorKind::Unsupported)), + } + } + + + /// Reads raw register bytes from multiple ids at once. /// /// Sends a sync read instruction to the specified motors and wait for the status packet in response. diff --git a/src/protocol/mod.rs b/src/protocol/mod.rs index 2c194b8..60f28b6 100644 --- a/src/protocol/mod.rs +++ b/src/protocol/mod.rs @@ -31,6 +31,12 @@ pub trait Protocol { self.send_instruction_packet(port, P::write_packet(id, addr, data).as_ref())?; self.read_status_packet(port, id).map(|_| ()) } + + fn write_fb(&self, port: &mut dyn SerialPort, id: u8, addr: u8, data: &[u8]) -> Result> { + self.send_instruction_packet(port, P::write_packet(id, addr, data).as_ref())?; + self.read_status_packet(port, id).map(|sp| sp.params().to_vec()) + } + fn sync_read( &self, port: &mut dyn SerialPort, @@ -120,6 +126,9 @@ pub enum CommunicationErrorKind { TimeoutError, /// Incorrect response id - different from sender (sender id, response id) IncorrectId(u8, u8), + + /// Operation not supported + Unsupported, } impl fmt::Display for CommunicationErrorKind { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { @@ -130,6 +139,7 @@ impl fmt::Display for CommunicationErrorKind { CommunicationErrorKind::IncorrectId(sender_id, resp_id) => { write!(f, "Incorrect id ({} instead of {})", resp_id, sender_id) } + CommunicationErrorKind::Unsupported => write!(f, "Operation not supported"), } } } From d0813d0818cf863974ee3aadfffc537fa8353942 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 2 Jan 2024 19:10:50 +0100 Subject: [PATCH 21/52] change name --- examples/poulpe3d.rs | 30 +++++++++---------- src/device/mod.rs | 2 +- .../{poulpe3d.rs => orbita3d_poulpe.rs} | 2 +- 3 files changed, 16 insertions(+), 18 deletions(-) rename src/device/{poulpe3d.rs => orbita3d_poulpe.rs} (98%) diff --git a/examples/poulpe3d.rs b/examples/poulpe3d.rs index f346c1b..0e48695 100644 --- a/examples/poulpe3d.rs +++ b/examples/poulpe3d.rs @@ -2,7 +2,7 @@ use std::f32::consts::PI; use std::time::SystemTime; use std::{error::Error, thread, time::Duration, time::Instant}; -use rustypot::device::poulpe3d::{self, MotorValue}; +use rustypot::device::orbita3d_poulpe::{self, MotorValue}; use rustypot::DynamixelSerialIO; use clap::Parser; @@ -11,14 +11,14 @@ use clap::Parser; #[command(author, version, about, long_about = None)] struct Args { /// tty - #[arg(short, long)] + #[arg(short, long, default_value = "/dev/ttyUSB0")] serialport: String, /// baud - #[arg(short, long, default_value_t = 1_000_000)] + #[arg(short, long, default_value_t = 2_000_000)] baudrate: u32, /// id - #[arg(short, long)] + #[arg(short, long, default_value_t = 42)] id: u8, ///sinus amplitude (f64) @@ -30,7 +30,7 @@ struct Args { frequency: f32, } -const MOTOR_A: u8 = 42; + fn main() -> Result<(), Box> { let args = Args::parse(); @@ -61,12 +61,16 @@ fn main() -> Result<(), Box> { let x = io.ping(serial_port.as_mut(), id); println!("Ping : {:?}", x); - let _ = poulpe3d::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:true, middle:true, bottom:true})?; + let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:true, middle:true, bottom:true})?; thread::sleep(Duration::from_millis(1000)); - let torque = poulpe3d::read_torque_enable(&io, serial_port.as_mut(), id)?; + 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= orbita3d_poulpe::read_current_position(&io, serial_port.as_mut(), id)?; + + let mut t = now.elapsed().unwrap().as_secs_f32(); loop { let t0 = Instant::now(); @@ -81,7 +85,7 @@ fn main() -> Result<(), Box> { t = now.elapsed().unwrap().as_secs_f32(); let target = amplitude * 180.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin(); - let feedback = poulpe3d::write_target_position(&io, serial_port.as_mut(), id, MotorValue::{top:target, middle:target, bottom:target})?; + let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), id, MotorValue::{top:target+curr_pos.top, middle:target+curr_pos.middle, bottom:target+curr_pos.bottom})?; println!("target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); @@ -91,14 +95,8 @@ fn main() -> Result<(), Box> { 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"); - // } - let _ = poulpe3d::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:false, middle:false, bottom:false})?; + + let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:false, middle:false, bottom:false})?; thread::sleep(Duration::from_millis(2000)); diff --git a/src/device/mod.rs b/src/device/mod.rs index e2ad681..edadac0 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -141,4 +141,4 @@ pub mod orbita2dof_foc; pub mod orbita_foc; pub mod xl320; -pub mod poulpe3d; +pub mod orbita3d_poulpe; diff --git a/src/device/poulpe3d.rs b/src/device/orbita3d_poulpe.rs similarity index 98% rename from src/device/poulpe3d.rs rename to src/device/orbita3d_poulpe.rs index 6215458..6084b22 100644 --- a/src/device/poulpe3d.rs +++ b/src/device/orbita3d_poulpe.rs @@ -1,4 +1,4 @@ -//! Orbita 2DoF Serial SimpleFOC register (protocol v1) +//! Orbita 3Dof Poulpe version use crate::device::*; From 4abeec6dacf6fddf6de237fa34b37cf6675e390e Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Fri, 5 Jan 2024 15:27:26 +0100 Subject: [PATCH 22/52] add poulpe2d --- examples/poulpe2d.rs | 128 ++++++++++++++++++++++++++++++++++ examples/poulpe3d.rs | 82 ++++++++++++++++++---- src/device/orbita2d_poulpe.rs | 1 + src/device/orbita3d_poulpe.rs | 22 +++++- 4 files changed, 220 insertions(+), 13 deletions(-) create mode 100644 examples/poulpe2d.rs diff --git a/examples/poulpe2d.rs b/examples/poulpe2d.rs new file mode 100644 index 0000000..f6b8819 --- /dev/null +++ b/examples/poulpe2d.rs @@ -0,0 +1,128 @@ +use std::f32::consts::PI; +use std::time::SystemTime; +use std::{error::Error, thread, time::Duration, time::Instant}; + +use rustypot::device::orbita2d_poulpe::{self,MotorPositionSpeedLoad, 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 = 42)] + 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> { + 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::{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)?; + + + // 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; + + loop { + let t0 = Instant::now(); + + if t > 10.0 { + break; + } + + + t = now.elapsed().unwrap().as_secs_f32(); + let target = amplitude * 180.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin(); + + let feedback = orbita2d_poulpe::write_target_position(&io, serial_port.as_mut(), id, MotorValue::{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: {} {} feedback vel: {} {} feedback torque: {} {}", target, feedback.position.motor_a,feedback.position.motor_b,feedback.speed.motor_a,feedback.speed.motor_b,feedback.load.motor_a,feedback.load.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::{motor_a:false, motor_b:false})?; + + + thread::sleep(Duration::from_millis(2000)); + + Ok(()) +} diff --git a/examples/poulpe3d.rs b/examples/poulpe3d.rs index 0e48695..b301b70 100644 --- a/examples/poulpe3d.rs +++ b/examples/poulpe3d.rs @@ -2,6 +2,7 @@ use std::f32::consts::PI; use std::time::SystemTime; use std::{error::Error, thread, time::Duration, time::Instant}; +use rustypot::device::orbita2d_poulpe::MotorPositionSpeedLoad; use rustypot::device::orbita3d_poulpe::{self, MotorValue}; use rustypot::DynamixelSerialIO; @@ -50,7 +51,7 @@ fn main() -> Result<(), Box> { println!("frequency: {}", frequency); let mut serial_port = serialport::new(serialportname, baudrate) - .timeout(Duration::from_millis(100)) + .timeout(Duration::from_millis(10)) .open()?; let now = SystemTime::now(); @@ -59,19 +60,26 @@ fn main() -> Result<(), Box> { // Ping let x = io.ping(serial_port.as_mut(), id); - println!("Ping : {:?}", x); + println!("Ping {:?}: {:?}", id,x); let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:true, middle:true, bottom: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 torque = orbita3d_poulpe::read_torque_enable(&io, serial_port.as_mut(), id)?; + // println!("torque: {:?}", torque); + // thread::sleep(Duration::from_millis(1000)); let curr_pos= orbita3d_poulpe::read_current_position(&io, serial_port.as_mut(), id)?; + // 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; + loop { let t0 = Instant::now(); @@ -79,23 +87,73 @@ fn main() -> Result<(), Box> { 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 feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), id, MotorValue::{top:target+curr_pos.top, middle:target+curr_pos.middle, bottom:target+curr_pos.bottom})?; + let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), id, MotorValue::{top:target+curr_pos.top, middle:target+curr_pos.middle, bottom:target+curr_pos.bottom}); + match feedback { + Ok(feedback) => { + nbok+=1; + println!("42 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); + }, + Err(e) => { + nberr+=1; + println!("error: {:?}", e); + } + } + + // thread::sleep(Duration::from_micros(500)); + // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 43, MotorValue::{top:0.0, middle:0.0, bottom:0.0}); + // // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 43, MotorValue::{top:0.0, middle:0.0, bottom:0.0}).unwrap_or_else(MotorPositionSpeedLoad::{position:MotorValue::{top:0.0, middle:0.0, bottom:0.0}, speed:MotorValue::{top:0.0, middle:0.0, bottom:0.0}, load:MotorValue::{top:0.0, middle:0.0, bottom:0.0}}); + // match feedback { + // Ok(feedback) => { + // nbok+=1; + // println!("43 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); + // }, + // Err(e) => { + // nberr+=1; + // println!("error: {:?}", e); + // } + // } + + + + // println!("43 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); + + // thread::sleep(Duration::from_micros(500)); + // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 44, MotorValue::{top:0.0, middle:0.0, bottom:0.0}); + // // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 44, MotorValue::{top:0.0, middle:0.0, bottom:0.0}).unwrap_or_else(MotorPositionSpeedLoad::{position:MotorValue::{top:0.0, middle:0.0, bottom:0.0}, speed:MotorValue::{top:0.0, middle:0.0, bottom:0.0}, load:MotorValue::{top:0.0, middle:0.0, bottom:0.0}}); + // match feedback { + // Ok(feedback) => { + // nbok+=1; + // println!("44 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); + // }, + // Err(e) => { + // nberr+=1; + // println!("error: {:?}", e); + // } + // } + + // nbtot+=3; + nbtot+=1; + // println!("44 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); + + // let axis_sensor = orbita3d_poulpe::read_axis_sensor(&io, serial_port.as_mut(), id)?; + // println!("axis_sensor: {:6.2} {:6.2} {:6.2}", axis_sensor.top, axis_sensor.middle, axis_sensor.bottom); + - println!("target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); - println!("ELAPSED: {:?}",t0.elapsed().as_micros()); // thread::sleep(Duration::from_micros(1000-t0.elapsed().as_micros() as u64)); - thread::sleep(Duration::from_millis(1)); + // 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 _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:false, middle:false, bottom:false})?; diff --git a/src/device/orbita2d_poulpe.rs b/src/device/orbita2d_poulpe.rs index 47472cb..761d3b9 100644 --- a/src/device/orbita2d_poulpe.rs +++ b/src/device/orbita2d_poulpe.rs @@ -46,6 +46,7 @@ reg_read_write_fb!(target_position, 60, MotorValue::,MotorPositionSpeedLoad reg_read_only!(axis_sensor, 90, MotorValue::); + reg_read_only!(full_state, 100, MotorPositionSpeedLoad); diff --git a/src/device/orbita3d_poulpe.rs b/src/device/orbita3d_poulpe.rs index 2c3dafb..e7c3bf4 100644 --- a/src/device/orbita3d_poulpe.rs +++ b/src/device/orbita3d_poulpe.rs @@ -55,7 +55,7 @@ reg_read_only!(current_torque, 52, MotorValue::); reg_read_write_fb!(target_position, 60, MotorValue::,MotorPositionSpeedLoad); reg_read_only!(axis_sensor, 90, MotorValue::); - +reg_read_only!(index_sensor, 99, MotorValue::); reg_read_only!(full_state, 100, MotorPositionSpeedLoad); @@ -128,6 +128,26 @@ impl MotorValue { } +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 3]) -> Self { + MotorValue { + top: bytes[0], + middle: bytes[1], + bottom: bytes[2], + } + } + pub fn to_le_bytes(&self) -> [u8; 3] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&[self.top]); + bytes.extend_from_slice(&[self.middle]); + bytes.extend_from_slice(&[self.bottom]); + + bytes.try_into().unwrap() + } +} + + impl MotorValue { pub fn from_le_bytes(bytes: [u8; 12]) -> Self { MotorValue { From 45eb741866a5c0347d720699f6c90fe416d36c5e Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 9 Jan 2024 18:02:43 +0100 Subject: [PATCH 23/52] limits types --- examples/poulpe2d.rs | 5 +- examples/poulpe3d.rs | 22 ++++++++- src/device/orbita2d_poulpe.rs | 87 ++++++++++++++++++++++++++++++--- src/device/orbita3d_poulpe.rs | 90 ++++++++++++++++++++++++++++++++--- 4 files changed, 188 insertions(+), 16 deletions(-) diff --git a/examples/poulpe2d.rs b/examples/poulpe2d.rs index f6b8819..2626f7e 100644 --- a/examples/poulpe2d.rs +++ b/examples/poulpe2d.rs @@ -19,7 +19,7 @@ struct Args { baudrate: u32, /// id - #[arg(short, long, default_value_t = 42)] + #[arg(short, long, default_value_t = 43)] id: u8, ///sinus amplitude (f64) @@ -63,7 +63,7 @@ fn main() -> Result<(), Box> { println!("Ping {:?}: {:?}", id,x); thread::sleep(Duration::from_millis(100)); - let _ = orbita2d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{motor_a:true, motor_b:true})?; + // let _ = orbita2d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{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); @@ -72,6 +72,7 @@ fn main() -> Result<(), Box> { 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); diff --git a/examples/poulpe3d.rs b/examples/poulpe3d.rs index b301b70..15b293b 100644 --- a/examples/poulpe3d.rs +++ b/examples/poulpe3d.rs @@ -62,7 +62,8 @@ fn main() -> Result<(), Box> { let x = io.ping(serial_port.as_mut(), id); println!("Ping {:?}: {:?}", id,x); - let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:true, middle:true, bottom:true})?; + // let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:true, middle:true, bottom:true})?; + let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:false, middle:false, bottom:false})?; thread::sleep(Duration::from_millis(1000)); // let torque = orbita3d_poulpe::read_torque_enable(&io, serial_port.as_mut(), id)?; // println!("torque: {:?}", torque); @@ -75,6 +76,25 @@ fn main() -> Result<(), Box> { // 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 limit= orbita3d_poulpe::read_velocity_limit(&io, serial_port.as_mut(), id)?; + println!("vel_limit: {:?} {:?} {:?}", limit.top, limit.middle, limit.bottom); + let limit= orbita3d_poulpe::read_torque_flux_limit(&io, serial_port.as_mut(), id)?; + println!("tf_limit: {:?} {:?} {:?}", limit.top, limit.middle, limit.bottom); + let limit= orbita3d_poulpe::read_uq_ud_limit(&io, serial_port.as_mut(), id)?; + println!("uq_ud_limit: {:?} {:?} {:?}", limit.top, limit.middle, limit.bottom); + + let pid =orbita3d_poulpe::read_flux_pid(&io, serial_port.as_mut(), id)?; + println!("flux_pid: {:?} {:?} {:?}", pid.top, pid.middle, pid.bottom); + let pid =orbita3d_poulpe::read_torque_pid(&io, serial_port.as_mut(), id)?; + println!("torque_pid: {:?} {:?} {:?}", pid.top, pid.middle, pid.bottom); + let pid =orbita3d_poulpe::read_velocity_pid(&io, serial_port.as_mut(), id)?; + println!("velocity_pid: {:?} {:?} {:?}", pid.top, pid.middle, pid.bottom); + let pid =orbita3d_poulpe::read_position_pid(&io, serial_port.as_mut(), id)?; + println!("position_pid: {:?} {:?} {:?}", pid.top, pid.middle, pid.bottom); + + + let mut t = now.elapsed().unwrap().as_secs_f32(); let mut nberr=0; let mut nbtot=0; diff --git a/src/device/orbita2d_poulpe.rs b/src/device/orbita2d_poulpe.rs index 761d3b9..c995711 100644 --- a/src/device/orbita2d_poulpe.rs +++ b/src/device/orbita2d_poulpe.rs @@ -27,9 +27,9 @@ reg_read_only!(model_number, 0, u16); reg_read_only!(firmware_version, 6, u8); reg_read_write!(id, 7, u8); -reg_read_write!(velocity_limit, 10, MotorValue::); -reg_read_write!(torque_flux_limit, 14, MotorValue::); -reg_read_write!(uq_ud_limit, 18, MotorValue::); +reg_read_write!(velocity_limit, 10, MotorValue::); +reg_read_write!(torque_flux_limit, 14, MotorValue::); +reg_read_write!(uq_ud_limit, 18, MotorValue::); reg_read_write!(flux_pid, 20, MotorValue::); reg_read_write!(torque_pid, 24, MotorValue::); @@ -52,11 +52,11 @@ reg_read_only!(full_state, 100, MotorPositionSpeedLoad); impl MotorPositionSpeedLoad { - pub fn from_le_bytes(bytes: [u8; 36]) -> Self { + pub fn from_le_bytes(bytes: [u8; 24]) -> Self { MotorPositionSpeedLoad { - position: MotorValue::::from_le_bytes(bytes[0..12].try_into().unwrap()), - speed: MotorValue::::from_le_bytes(bytes[12..24].try_into().unwrap()), - load: MotorValue::::from_le_bytes(bytes[24..36].try_into().unwrap()), + position: MotorValue::::from_le_bytes(bytes[0..8].try_into().unwrap()), + speed: MotorValue::::from_le_bytes(bytes[8..16].try_into().unwrap()), + load: MotorValue::::from_le_bytes(bytes[16..24].try_into().unwrap()), } } // pub fn to_le_bytes(&self) -> [u8; 36] { @@ -97,6 +97,79 @@ impl MotorValue { } + +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 8]) -> Self { + MotorValue { + motor_a: u32::from_le_bytes(bytes[0..4].try_into().unwrap()), + motor_b: u32::from_le_bytes(bytes[4..8].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 8] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.motor_a.to_le_bytes()); + bytes.extend_from_slice(&self.motor_b.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 8]) -> Self { + MotorValue { + motor_a: i32::from_le_bytes(bytes[0..4].try_into().unwrap()), + motor_b: i32::from_le_bytes(bytes[4..8].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 8] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.motor_a.to_le_bytes()); + bytes.extend_from_slice(&self.motor_b.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 4]) -> Self { + MotorValue { + motor_a: i16::from_le_bytes(bytes[0..2].try_into().unwrap()), + motor_b: i16::from_le_bytes(bytes[2..4].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 4] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.motor_a.to_le_bytes()); + bytes.extend_from_slice(&self.motor_b.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + + +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 4]) -> Self { + MotorValue { + motor_a: u16::from_le_bytes(bytes[0..2].try_into().unwrap()), + motor_b: u16::from_le_bytes(bytes[2..4].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 4] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.motor_a.to_le_bytes()); + bytes.extend_from_slice(&self.motor_b.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + + + + impl MotorValue { pub fn from_le_bytes(bytes: [u8; 2]) -> Self { MotorValue { diff --git a/src/device/orbita3d_poulpe.rs b/src/device/orbita3d_poulpe.rs index e7c3bf4..cd6c4e0 100644 --- a/src/device/orbita3d_poulpe.rs +++ b/src/device/orbita3d_poulpe.rs @@ -37,9 +37,9 @@ reg_read_only!(model_number, 0, u16); reg_read_only!(firmware_version, 6, u8); reg_read_write!(id, 7, u8); -reg_read_write!(velocity_limit, 10, MotorValue::); -reg_read_write!(torque_flux_limit, 14, MotorValue::); -reg_read_write!(uq_ud_limit, 18, MotorValue::); +reg_read_write!(velocity_limit, 10, MotorValue::); +reg_read_write!(torque_flux_limit, 14, MotorValue::); +reg_read_write!(uq_ud_limit, 18, MotorValue::); reg_read_write!(flux_pid, 20, MotorValue::); reg_read_write!(torque_pid, 24, MotorValue::); @@ -108,6 +108,84 @@ impl MotorValue { } +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 12]) -> Self { + MotorValue { + top: u32::from_le_bytes(bytes[0..4].try_into().unwrap()), + middle: u32::from_le_bytes(bytes[4..8].try_into().unwrap()), + bottom: u32::from_le_bytes(bytes[8..12].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 12] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.top.to_le_bytes()); + bytes.extend_from_slice(&self.middle.to_le_bytes()); + bytes.extend_from_slice(&self.bottom.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 12]) -> Self { + MotorValue { + top: i32::from_le_bytes(bytes[0..4].try_into().unwrap()), + middle: i32::from_le_bytes(bytes[4..8].try_into().unwrap()), + bottom: i32::from_le_bytes(bytes[8..12].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 12] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.top.to_le_bytes()); + bytes.extend_from_slice(&self.middle.to_le_bytes()); + bytes.extend_from_slice(&self.bottom.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 6]) -> Self { + MotorValue { + top: i16::from_le_bytes(bytes[0..2].try_into().unwrap()), + middle: i16::from_le_bytes(bytes[2..4].try_into().unwrap()), + bottom: i16::from_le_bytes(bytes[4..6].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 6] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.top.to_le_bytes()); + bytes.extend_from_slice(&self.middle.to_le_bytes()); + bytes.extend_from_slice(&self.bottom.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + + +impl MotorValue { + pub fn from_le_bytes(bytes: [u8; 6]) -> Self { + MotorValue { + top: u16::from_le_bytes(bytes[0..2].try_into().unwrap()), + middle: u16::from_le_bytes(bytes[2..4].try_into().unwrap()), + bottom: u16::from_le_bytes(bytes[4..6].try_into().unwrap()), + } + } + pub fn to_le_bytes(&self) -> [u8; 6] { + let mut bytes = Vec::new(); + + bytes.extend_from_slice(&self.top.to_le_bytes()); + bytes.extend_from_slice(&self.middle.to_le_bytes()); + bytes.extend_from_slice(&self.bottom.to_le_bytes()); + + bytes.try_into().unwrap() + } +} + + impl MotorValue { pub fn from_le_bytes(bytes: [u8; 3]) -> Self { MotorValue { @@ -196,15 +274,15 @@ impl Vec3d { impl Pid { pub fn from_le_bytes(bytes: [u8; 4]) -> Self { Pid { - p: i16::from_le_bytes(bytes[0..2].try_into().unwrap()), - i: i16::from_le_bytes(bytes[2..4].try_into().unwrap()), + i: i16::from_le_bytes(bytes[0..2].try_into().unwrap()), + p: i16::from_le_bytes(bytes[2..4].try_into().unwrap()), } } pub fn to_le_bytes(&self) -> [u8; 4] { let mut bytes = Vec::new(); - bytes.extend_from_slice(&self.p.to_le_bytes()); bytes.extend_from_slice(&self.i.to_le_bytes()); + bytes.extend_from_slice(&self.p.to_le_bytes()); bytes.try_into().unwrap() } From 54f833d76f1a3025651c9d65e24a3c75bfd2b43a Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 6 Feb 2024 17:27:36 +0100 Subject: [PATCH 24/52] update limits f32 --- examples/poulpe3d.rs | 6 ++-- src/device/orbita3d_poulpe.rs | 61 ++++++++++++++++------------------- 2 files changed, 30 insertions(+), 37 deletions(-) diff --git a/examples/poulpe3d.rs b/examples/poulpe3d.rs index 15b293b..512e59b 100644 --- a/examples/poulpe3d.rs +++ b/examples/poulpe3d.rs @@ -62,8 +62,8 @@ fn main() -> Result<(), Box> { let x = io.ping(serial_port.as_mut(), id); println!("Ping {:?}: {:?}", id,x); - // let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:true, middle:true, bottom:true})?; - let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:false, middle:false, bottom:false})?; + let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:true, middle:true, bottom:true})?; + // let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:false, middle:false, bottom:false})?; thread::sleep(Duration::from_millis(1000)); // let torque = orbita3d_poulpe::read_torque_enable(&io, serial_port.as_mut(), id)?; // println!("torque: {:?}", torque); @@ -103,7 +103,7 @@ fn main() -> Result<(), Box> { loop { let t0 = Instant::now(); - if t > 10.0 { + if t > 100.0 { break; } diff --git a/src/device/orbita3d_poulpe.rs b/src/device/orbita3d_poulpe.rs index cd6c4e0..580ca35 100644 --- a/src/device/orbita3d_poulpe.rs +++ b/src/device/orbita3d_poulpe.rs @@ -1,4 +1,3 @@ - //! Orbita 3Dof Poulpe version use crate::device::*; @@ -37,9 +36,9 @@ reg_read_only!(model_number, 0, u16); reg_read_only!(firmware_version, 6, u8); reg_read_write!(id, 7, u8); -reg_read_write!(velocity_limit, 10, MotorValue::); -reg_read_write!(torque_flux_limit, 14, MotorValue::); -reg_read_write!(uq_ud_limit, 18, MotorValue::); +reg_read_write!(velocity_limit, 10, MotorValue::); +reg_read_write!(torque_flux_limit, 14, MotorValue::); +reg_read_write!(uq_ud_limit, 18, MotorValue::); reg_read_write!(flux_pid, 20, MotorValue::); reg_read_write!(torque_pid, 24, MotorValue::); @@ -52,35 +51,35 @@ reg_read_only!(current_position, 50, MotorValue::); reg_read_only!(current_velocity, 51, MotorValue::); reg_read_only!(current_torque, 52, MotorValue::); -reg_read_write_fb!(target_position, 60, MotorValue::,MotorPositionSpeedLoad); +reg_read_write_fb!( + target_position, + 60, + MotorValue::, + MotorPositionSpeedLoad +); reg_read_only!(axis_sensor, 90, MotorValue::); reg_read_only!(index_sensor, 99, MotorValue::); reg_read_only!(full_state, 100, MotorPositionSpeedLoad); - - impl MotorPositionSpeedLoad { - pub fn from_le_bytes(bytes: [u8; 36]) -> Self { - MotorPositionSpeedLoad { - position: MotorValue::::from_le_bytes(bytes[0..12].try_into().unwrap()), - speed: MotorValue::::from_le_bytes(bytes[12..24].try_into().unwrap()), - load: MotorValue::::from_le_bytes(bytes[24..36].try_into().unwrap()), - } - } - // pub fn to_le_bytes(&self) -> [u8; 36] { - // let mut bytes = Vec::new(); - - // bytes.extend_from_slice(&self.position.to_le_bytes()); - // bytes.extend_from_slice(&self.speed.to_le_bytes()); - // bytes.extend_from_slice(&self.load.to_le_bytes()); - - // bytes.try_into().unwrap() - // } -} - + pub fn from_le_bytes(bytes: [u8; 36]) -> Self { + MotorPositionSpeedLoad { + position: MotorValue::::from_le_bytes(bytes[0..12].try_into().unwrap()), + speed: MotorValue::::from_le_bytes(bytes[12..24].try_into().unwrap()), + load: MotorValue::::from_le_bytes(bytes[24..36].try_into().unwrap()), + } + } + // pub fn to_le_bytes(&self) -> [u8; 36] { + // let mut bytes = Vec::new(); + // bytes.extend_from_slice(&self.position.to_le_bytes()); + // bytes.extend_from_slice(&self.speed.to_le_bytes()); + // bytes.extend_from_slice(&self.load.to_le_bytes()); + // bytes.try_into().unwrap() + // } +} impl PartialEq for MotorValue { fn eq(&self, other: &Self) -> bool { @@ -107,7 +106,6 @@ impl MotorValue { } } - impl MotorValue { pub fn from_le_bytes(bytes: [u8; 12]) -> Self { MotorValue { @@ -165,7 +163,6 @@ impl MotorValue { } } - impl MotorValue { pub fn from_le_bytes(bytes: [u8; 6]) -> Self { MotorValue { @@ -185,13 +182,12 @@ impl MotorValue { } } - impl MotorValue { pub fn from_le_bytes(bytes: [u8; 3]) -> Self { MotorValue { - top: bytes[0] !=0, - middle: bytes[1] !=0, - bottom: bytes[2] !=0, + top: bytes[0] != 0, + middle: bytes[1] != 0, + bottom: bytes[2] != 0, } } pub fn to_le_bytes(&self) -> [u8; 3] { @@ -205,7 +201,6 @@ impl MotorValue { } } - impl MotorValue { pub fn from_le_bytes(bytes: [u8; 3]) -> Self { MotorValue { @@ -225,7 +220,6 @@ impl MotorValue { } } - impl MotorValue { pub fn from_le_bytes(bytes: [u8; 12]) -> Self { MotorValue { @@ -245,7 +239,6 @@ impl MotorValue { } } - impl PartialEq for Vec3d { fn eq(&self, other: &Self) -> bool { self.x == other.x && self.y == other.y && self.z == other.z From 4a42c4fb6c8c8aaf3d6dd8c3f45200ba8be7b216 Mon Sep 17 00:00:00 2001 From: Antun Skuric <36178713+askuric@users.noreply.github.com> Date: Thu, 8 Feb 2024 14:05:19 +0100 Subject: [PATCH 25/52] Update orbita2d_poulpe.rs --- src/device/orbita2d_poulpe.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/device/orbita2d_poulpe.rs b/src/device/orbita2d_poulpe.rs index c995711..f7cd4ac 100644 --- a/src/device/orbita2d_poulpe.rs +++ b/src/device/orbita2d_poulpe.rs @@ -27,8 +27,8 @@ reg_read_only!(model_number, 0, u16); reg_read_only!(firmware_version, 6, u8); reg_read_write!(id, 7, u8); -reg_read_write!(velocity_limit, 10, MotorValue::); -reg_read_write!(torque_flux_limit, 14, MotorValue::); +reg_read_write!(velocity_limit, 10, MotorValue::); +reg_read_write!(torque_flux_limit, 14, MotorValue::); reg_read_write!(uq_ud_limit, 18, MotorValue::); reg_read_write!(flux_pid, 20, MotorValue::); From a99b5703c65f7816a0fad631bc4f46df56429011 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Mon, 19 Feb 2024 17:01:06 +0100 Subject: [PATCH 26/52] only pos in fb --- examples/poulpe2d.rs | 122 +++++++++++++++++++++++----------- src/bin/dxl_scan.rs | 2 +- src/device/orbita3d_poulpe.rs | 8 +-- 3 files changed, 88 insertions(+), 44 deletions(-) diff --git a/examples/poulpe2d.rs b/examples/poulpe2d.rs index 2626f7e..18e2c06 100644 --- a/examples/poulpe2d.rs +++ b/examples/poulpe2d.rs @@ -2,7 +2,7 @@ use std::f32::consts::PI; use std::time::SystemTime; use std::{error::Error, thread, time::Duration, time::Instant}; -use rustypot::device::orbita2d_poulpe::{self,MotorPositionSpeedLoad, MotorValue}; +use rustypot::device::orbita2d_poulpe::{self, MotorPositionSpeedLoad, MotorValue}; // use rustypot::device::orbita3d_poulpe::{self, MotorValue}; use rustypot::DynamixelSerialIO; @@ -23,7 +23,7 @@ struct Args { id: u8, ///sinus amplitude (f64) - #[arg(short, long, default_value_t = 10.0)] + #[arg(short, long, default_value_t = 1.0)] amplitude: f32, ///sinus frequency (f64) @@ -31,8 +31,6 @@ struct Args { frequency: f32, } - - fn main() -> Result<(), Box> { let args = Args::parse(); let serialportname: String = args.serialport; @@ -60,17 +58,24 @@ fn main() -> Result<(), Box> { // Ping let x = io.ping(serial_port.as_mut(), id); - println!("Ping {:?}: {:?}", id,x); + println!("Ping {:?}: {:?}", id, x); thread::sleep(Duration::from_millis(100)); - // let _ = orbita2d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{motor_a:true, motor_b:true})?; + let _ = orbita2d_poulpe::write_torque_enable( + &io, + serial_port.as_mut(), + id, + MotorValue:: { + 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)?; + 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); @@ -78,50 +83,89 @@ fn main() -> Result<(), Box> { // 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 nberr = 0; + let mut nbtot = 0; + let mut nbok = 0; + let mut target = 0.0; loop { - let t0 = Instant::now(); + let t0 = Instant::now(); - if t > 10.0 { + 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(); - - let feedback = orbita2d_poulpe::write_target_position(&io, serial_port.as_mut(), id, MotorValue::{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: {} {} feedback vel: {} {} feedback torque: {} {}", target, feedback.position.motor_a,feedback.position.motor_b,feedback.speed.motor_a,feedback.speed.motor_b,feedback.load.motor_a,feedback.load.motor_b); - }, - Err(e) => { - nberr+=1; - println!("error: {:?}", e); - } - } - - nbtot+=1; - - + // 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::{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:: { + // 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:: { + 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: {} {} feedback vel: {} {} feedback torque: {} {}", + target, + feedback.position.motor_a, + feedback.position.motor_b, + feedback.speed.motor_a, + feedback.speed.motor_b, + feedback.load.motor_a, + feedback.load.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_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()); + 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!( + "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::{motor_a:false, motor_b:false})?; - + let _ = orbita2d_poulpe::write_torque_enable( + &io, + serial_port.as_mut(), + id, + MotorValue:: { + motor_a: false, + motor_b: false, + }, + )?; thread::sleep(Duration::from_millis(2000)); diff --git a/src/bin/dxl_scan.rs b/src/bin/dxl_scan.rs index baaf3d9..23bc5eb 100644 --- a/src/bin/dxl_scan.rs +++ b/src/bin/dxl_scan.rs @@ -20,7 +20,7 @@ fn main() -> Result<(), Box> { let args = Args::parse(); println!("Scanning..."); - let mut serial_port = serialport::new(args.serial_port, 1_000_000) + let mut serial_port = serialport::new(args.serial_port, 2_000_000) .timeout(Duration::from_millis(10)) .open()?; diff --git a/src/device/orbita3d_poulpe.rs b/src/device/orbita3d_poulpe.rs index 580ca35..87f2d9d 100644 --- a/src/device/orbita3d_poulpe.rs +++ b/src/device/orbita3d_poulpe.rs @@ -22,8 +22,8 @@ pub struct Vec3d { #[derive(Clone, Copy, Debug)] pub struct MotorPositionSpeedLoad { pub position: MotorValue, - pub speed: MotorValue, - pub load: MotorValue, + // pub speed: MotorValue, + // pub load: MotorValue, } /// Wrapper for PID gains. #[derive(Clone, Copy, Debug, PartialEq)] @@ -66,8 +66,8 @@ impl MotorPositionSpeedLoad { pub fn from_le_bytes(bytes: [u8; 36]) -> Self { MotorPositionSpeedLoad { position: MotorValue::::from_le_bytes(bytes[0..12].try_into().unwrap()), - speed: MotorValue::::from_le_bytes(bytes[12..24].try_into().unwrap()), - load: MotorValue::::from_le_bytes(bytes[24..36].try_into().unwrap()), + // speed: MotorValue::::from_le_bytes(bytes[12..24].try_into().unwrap()), + // load: MotorValue::::from_le_bytes(bytes[24..36].try_into().unwrap()), } } // pub fn to_le_bytes(&self) -> [u8; 36] { From ca13b53c1409e6cf43146ea19d1db983ce733644 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Mon, 19 Feb 2024 17:12:18 +0100 Subject: [PATCH 27/52] wip --- src/device/orbita3d_poulpe.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/device/orbita3d_poulpe.rs b/src/device/orbita3d_poulpe.rs index 87f2d9d..1bb3d50 100644 --- a/src/device/orbita3d_poulpe.rs +++ b/src/device/orbita3d_poulpe.rs @@ -63,7 +63,7 @@ reg_read_only!(index_sensor, 99, MotorValue::); reg_read_only!(full_state, 100, MotorPositionSpeedLoad); impl MotorPositionSpeedLoad { - pub fn from_le_bytes(bytes: [u8; 36]) -> Self { + pub fn from_le_bytes(bytes: [u8; 12]) -> Self { MotorPositionSpeedLoad { position: MotorValue::::from_le_bytes(bytes[0..12].try_into().unwrap()), // speed: MotorValue::::from_le_bytes(bytes[12..24].try_into().unwrap()), From bda0a6570286ba44d08a730629a16ee9d6415b2f Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Mon, 19 Feb 2024 20:43:40 +0100 Subject: [PATCH 28/52] Add post delay option. --- examples/foc.rs | 2 +- src/lib.rs | 23 ++++++++++++++++++++++- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/examples/foc.rs b/examples/foc.rs index 789b6fd..72b988a 100644 --- a/examples/foc.rs +++ b/examples/foc.rs @@ -10,7 +10,7 @@ fn main() -> Result<(), Box> { .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)?; diff --git a/src/lib.rs b/src/lib.rs index 73309aa..138d7dd 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -28,6 +28,8 @@ //! ``` mod protocol; +use std::time::Duration; + pub use protocol::CommunicationErrorKind; use protocol::{Protocol, V1, V2}; @@ -48,6 +50,7 @@ enum Protocols { /// Raw dynamixel communication messages controller (protocol v1 or v2) pub struct DynamixelSerialIO { protocol: Protocols, + post_delay: Option, } impl DynamixelSerialIO { @@ -74,6 +77,7 @@ impl DynamixelSerialIO { pub fn v1() -> Self { DynamixelSerialIO { protocol: Protocols::V1(V1), + post_delay: None, } } /// Creates a protocol v2 communication IO. @@ -99,6 +103,15 @@ impl DynamixelSerialIO { pub fn v2() -> Self { DynamixelSerialIO { protocol: Protocols::V2(V2), + post_delay: None, + } + } + + /// Set a delay after each communication. + pub fn with_post_delay(self, delay: Duration) -> Self { + DynamixelSerialIO { + post_delay: Some(delay), + ..self } } @@ -172,10 +185,14 @@ impl DynamixelSerialIO { addr: u8, length: u8, ) -> Result> { - match &self.protocol { + let res = match &self.protocol { Protocols::V1(p) => p.read(serial_port, id, addr, length), Protocols::V2(p) => p.read(serial_port, id, addr, length), + }; + if let Some(delay) = self.post_delay { + std::thread::sleep(delay); } + res } /// Writes raw bytes to register. @@ -217,7 +234,11 @@ impl DynamixelSerialIO { match &self.protocol { Protocols::V1(p) => p.write(serial_port, id, addr, data), Protocols::V2(p) => p.write(serial_port, id, addr, data), + }?; + if let Some(delay) = self.post_delay { + std::thread::sleep(delay); } + Ok(()) } /// Reads raw register bytes from multiple ids at once. From 5f7792c4c075021c1b4c126eb91264303337dd8f Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Mon, 19 Feb 2024 20:45:47 +0100 Subject: [PATCH 29/52] Update Cargo.toml --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 2636469..2eaa3fa 100644 --- a/Cargo.toml +++ b/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"] From e57eaa045e2665a75569e328b1613120106c67dd Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Mon, 19 Feb 2024 20:46:36 +0100 Subject: [PATCH 30/52] Update Changelog.md --- Changelog.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Changelog.md b/Changelog.md index 6486da7..f2eae5c 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,3 +1,7 @@ +## Version 0.5.0 + +- Add an post delay option to the read and write method. + ## Version 0.4.0 - Add support for orbita-2dof-foc device. From f20677ea52a47674ae389e00f17764a51ddf433e Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 20 Feb 2024 11:24:04 +0100 Subject: [PATCH 31/52] 2dof --- src/device/orbita2d_poulpe.rs | 73 ++++++++++++----------------------- 1 file changed, 25 insertions(+), 48 deletions(-) diff --git a/src/device/orbita2d_poulpe.rs b/src/device/orbita2d_poulpe.rs index c995711..8678da8 100644 --- a/src/device/orbita2d_poulpe.rs +++ b/src/device/orbita2d_poulpe.rs @@ -10,8 +10,8 @@ pub struct MotorValue { #[derive(Clone, Copy, Debug)] pub struct MotorPositionSpeedLoad { pub position: MotorValue, - pub speed: MotorValue, - pub load: MotorValue, + // pub speed: MotorValue, + // pub load: MotorValue, } /// Wrapper for PID gains. #[derive(Clone, Copy, Debug, PartialEq)] @@ -20,9 +20,6 @@ pub struct Pid { pub i: i16, } - - - reg_read_only!(model_number, 0, u16); reg_read_only!(firmware_version, 6, u8); reg_read_write!(id, 7, u8); @@ -42,36 +39,35 @@ reg_read_only!(current_position, 50, MotorValue::); reg_read_only!(current_velocity, 51, MotorValue::); reg_read_only!(current_torque, 52, MotorValue::); -reg_read_write_fb!(target_position, 60, MotorValue::,MotorPositionSpeedLoad); +reg_read_write_fb!( + target_position, + 60, + MotorValue::, + MotorPositionSpeedLoad +); reg_read_only!(axis_sensor, 90, MotorValue::); - reg_read_only!(full_state, 100, MotorPositionSpeedLoad); - - impl MotorPositionSpeedLoad { - pub fn from_le_bytes(bytes: [u8; 24]) -> Self { - MotorPositionSpeedLoad { - position: MotorValue::::from_le_bytes(bytes[0..8].try_into().unwrap()), - speed: MotorValue::::from_le_bytes(bytes[8..16].try_into().unwrap()), - load: MotorValue::::from_le_bytes(bytes[16..24].try_into().unwrap()), - } - } - // pub fn to_le_bytes(&self) -> [u8; 36] { - // let mut bytes = Vec::new(); - - // bytes.extend_from_slice(&self.position.to_le_bytes()); - // bytes.extend_from_slice(&self.speed.to_le_bytes()); - // bytes.extend_from_slice(&self.load.to_le_bytes()); - - // bytes.try_into().unwrap() - // } -} - + pub fn from_le_bytes(bytes: [u8; 8]) -> Self { + MotorPositionSpeedLoad { + position: MotorValue::::from_le_bytes(bytes[0..8].try_into().unwrap()), + // speed: MotorValue::::from_le_bytes(bytes[8..16].try_into().unwrap()), + // load: MotorValue::::from_le_bytes(bytes[16..24].try_into().unwrap()), + } + } + // pub fn to_le_bytes(&self) -> [u8; 36] { + // let mut bytes = Vec::new(); + // bytes.extend_from_slice(&self.position.to_le_bytes()); + // bytes.extend_from_slice(&self.speed.to_le_bytes()); + // bytes.extend_from_slice(&self.load.to_le_bytes()); + // bytes.try_into().unwrap() + // } +} impl PartialEq for MotorValue { fn eq(&self, other: &Self) -> bool { @@ -96,8 +92,6 @@ impl MotorValue { } } - - impl MotorValue { pub fn from_le_bytes(bytes: [u8; 8]) -> Self { MotorValue { @@ -149,7 +143,6 @@ impl MotorValue { } } - impl MotorValue { pub fn from_le_bytes(bytes: [u8; 4]) -> Self { MotorValue { @@ -167,14 +160,11 @@ impl MotorValue { } } - - - impl MotorValue { pub fn from_le_bytes(bytes: [u8; 2]) -> Self { MotorValue { - motor_a: bytes[0] !=0, - motor_b: bytes[1] !=0, + motor_a: bytes[0] != 0, + motor_b: bytes[1] != 0, } } pub fn to_le_bytes(&self) -> [u8; 2] { @@ -187,7 +177,6 @@ impl MotorValue { } } - impl MotorValue { pub fn from_le_bytes(bytes: [u8; 8]) -> Self { MotorValue { @@ -205,7 +194,6 @@ impl MotorValue { } } - impl Pid { pub fn from_le_bytes(bytes: [u8; 4]) -> Self { Pid { @@ -223,17 +211,6 @@ impl Pid { } } - - - - - - - - - - - // /////////////////// // reg_read_write!(torque_enable, 40, MotorValue::); // reg_read_write!(present_position, 50, MotorValue::); From c47edacbf8fee1066741ae86ecd5e6c1879f9baa Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 20 Feb 2024 11:31:26 +0100 Subject: [PATCH 32/52] 2dof --- src/device/orbita2d_poulpe.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/device/orbita2d_poulpe.rs b/src/device/orbita2d_poulpe.rs index 8678da8..8de6720 100644 --- a/src/device/orbita2d_poulpe.rs +++ b/src/device/orbita2d_poulpe.rs @@ -24,8 +24,8 @@ reg_read_only!(model_number, 0, u16); reg_read_only!(firmware_version, 6, u8); reg_read_write!(id, 7, u8); -reg_read_write!(velocity_limit, 10, MotorValue::); -reg_read_write!(torque_flux_limit, 14, MotorValue::); +reg_read_write!(velocity_limit, 10, MotorValue::); +reg_read_write!(torque_flux_limit, 14, MotorValue::); reg_read_write!(uq_ud_limit, 18, MotorValue::); reg_read_write!(flux_pid, 20, MotorValue::); From c8093e04c6402212b0767128eae67106b804b0c8 Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Wed, 21 Feb 2024 15:36:41 +0100 Subject: [PATCH 33/52] Add post delay in write_fb --- src/lib.rs | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/lib.rs b/src/lib.rs index 4988307..95980a6 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -241,8 +241,6 @@ impl DynamixelSerialIO { Ok(()) } - - pub fn write_fb( &self, serial_port: &mut dyn serialport::SerialPort, @@ -251,13 +249,17 @@ impl DynamixelSerialIO { data: &[u8], ) -> Result> { match &self.protocol { - Protocols::V1(p) => p.write_fb(serial_port, id, addr, data), - Protocols::V2(p) => Err(Box::new(CommunicationErrorKind::Unsupported)), + Protocols::V1(p) => { + let res = p.write_fb(serial_port, id, addr, data); + if let Some(delay) = self.post_delay { + std::thread::sleep(delay); + } + res + } + Protocols::V2(_) => Err(Box::new(CommunicationErrorKind::Unsupported)), } } - - /// Reads raw register bytes from multiple ids at once. /// /// Sends a sync read instruction to the specified motors and wait for the status packet in response. From cf718a889aa82ead9ebd4a2393c1c656bf0600c8 Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Wed, 21 Feb 2024 15:47:38 +0100 Subject: [PATCH 34/52] Fix lint. --- examples/poulpe.rs | 96 -------------------- examples/poulpe2d.rs | 13 +-- examples/poulpe3d.rs | 205 ++++++++++++++++++++++++++----------------- src/protocol/mod.rs | 13 ++- 4 files changed, 138 insertions(+), 189 deletions(-) delete mode 100644 examples/poulpe.rs diff --git a/examples/poulpe.rs b/examples/poulpe.rs deleted file mode 100644 index 8877ecf..0000000 --- a/examples/poulpe.rs +++ /dev/null @@ -1,96 +0,0 @@ -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> { - 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(()) -} diff --git a/examples/poulpe2d.rs b/examples/poulpe2d.rs index 18e2c06..946060d 100644 --- a/examples/poulpe2d.rs +++ b/examples/poulpe2d.rs @@ -1,8 +1,7 @@ -use std::f32::consts::PI; use std::time::SystemTime; use std::{error::Error, thread, time::Duration, time::Instant}; -use rustypot::device::orbita2d_poulpe::{self, MotorPositionSpeedLoad, MotorValue}; +use rustypot::device::orbita2d_poulpe::{self, MotorValue}; // use rustypot::device::orbita3d_poulpe::{self, MotorValue}; use rustypot::DynamixelSerialIO; @@ -123,14 +122,8 @@ fn main() -> Result<(), Box> { Ok(feedback) => { nbok += 1; println!( - "42 target: {} feedback pos: {} {} feedback vel: {} {} feedback torque: {} {}", - target, - feedback.position.motor_a, - feedback.position.motor_b, - feedback.speed.motor_a, - feedback.speed.motor_b, - feedback.load.motor_a, - feedback.load.motor_b + "42 target: {} feedback pos: {} {}", + target, feedback.position.motor_a, feedback.position.motor_b, ); } Err(e) => { diff --git a/examples/poulpe3d.rs b/examples/poulpe3d.rs index 512e59b..ea84e3d 100644 --- a/examples/poulpe3d.rs +++ b/examples/poulpe3d.rs @@ -2,7 +2,6 @@ use std::f32::consts::PI; use std::time::SystemTime; use std::{error::Error, thread, time::Duration, time::Instant}; -use rustypot::device::orbita2d_poulpe::MotorPositionSpeedLoad; use rustypot::device::orbita3d_poulpe::{self, MotorValue}; use rustypot::DynamixelSerialIO; @@ -31,8 +30,6 @@ struct Args { frequency: f32, } - - fn main() -> Result<(), Box> { let args = Args::parse(); let serialportname: String = args.serialport; @@ -60,48 +57,70 @@ fn main() -> Result<(), Box> { // Ping let x = io.ping(serial_port.as_mut(), id); - println!("Ping {:?}: {:?}", id,x); - - let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:true, middle:true, bottom:true})?; + println!("Ping {:?}: {:?}", id, x); + + let _ = orbita3d_poulpe::write_torque_enable( + &io, + serial_port.as_mut(), + id, + MotorValue:: { + top: true, + middle: true, + bottom: true, + }, + )?; // let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:false, middle:false, bottom:false})?; 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= orbita3d_poulpe::read_current_position(&io, serial_port.as_mut(), id)?; - + let curr_pos = orbita3d_poulpe::read_current_position(&io, serial_port.as_mut(), id)?; // 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 limit= orbita3d_poulpe::read_velocity_limit(&io, serial_port.as_mut(), id)?; - println!("vel_limit: {:?} {:?} {:?}", limit.top, limit.middle, limit.bottom); - let limit= orbita3d_poulpe::read_torque_flux_limit(&io, serial_port.as_mut(), id)?; - println!("tf_limit: {:?} {:?} {:?}", limit.top, limit.middle, limit.bottom); - let limit= orbita3d_poulpe::read_uq_ud_limit(&io, serial_port.as_mut(), id)?; - println!("uq_ud_limit: {:?} {:?} {:?}", limit.top, limit.middle, limit.bottom); - - let pid =orbita3d_poulpe::read_flux_pid(&io, serial_port.as_mut(), id)?; + let limit = orbita3d_poulpe::read_velocity_limit(&io, serial_port.as_mut(), id)?; + println!( + "vel_limit: {:?} {:?} {:?}", + limit.top, limit.middle, limit.bottom + ); + let limit = orbita3d_poulpe::read_torque_flux_limit(&io, serial_port.as_mut(), id)?; + println!( + "tf_limit: {:?} {:?} {:?}", + limit.top, limit.middle, limit.bottom + ); + let limit = orbita3d_poulpe::read_uq_ud_limit(&io, serial_port.as_mut(), id)?; + println!( + "uq_ud_limit: {:?} {:?} {:?}", + limit.top, limit.middle, limit.bottom + ); + + let pid = orbita3d_poulpe::read_flux_pid(&io, serial_port.as_mut(), id)?; println!("flux_pid: {:?} {:?} {:?}", pid.top, pid.middle, pid.bottom); - let pid =orbita3d_poulpe::read_torque_pid(&io, serial_port.as_mut(), id)?; - println!("torque_pid: {:?} {:?} {:?}", pid.top, pid.middle, pid.bottom); - let pid =orbita3d_poulpe::read_velocity_pid(&io, serial_port.as_mut(), id)?; - println!("velocity_pid: {:?} {:?} {:?}", pid.top, pid.middle, pid.bottom); - let pid =orbita3d_poulpe::read_position_pid(&io, serial_port.as_mut(), id)?; - println!("position_pid: {:?} {:?} {:?}", pid.top, pid.middle, pid.bottom); - - + let pid = orbita3d_poulpe::read_torque_pid(&io, serial_port.as_mut(), id)?; + println!( + "torque_pid: {:?} {:?} {:?}", + pid.top, pid.middle, pid.bottom + ); + let pid = orbita3d_poulpe::read_velocity_pid(&io, serial_port.as_mut(), id)?; + println!( + "velocity_pid: {:?} {:?} {:?}", + pid.top, pid.middle, pid.bottom + ); + let pid = orbita3d_poulpe::read_position_pid(&io, serial_port.as_mut(), id)?; + println!( + "position_pid: {:?} {:?} {:?}", + pid.top, pid.middle, pid.bottom + ); let mut t = now.elapsed().unwrap().as_secs_f32(); - let mut nberr=0; - let mut nbtot=0; - let mut nbok=0; + let mut nberr = 0; + let mut nbtot = 0; + let mut nbok = 0; loop { - let t0 = Instant::now(); + let t0 = Instant::now(); if t > 100.0 { break; @@ -110,72 +129,96 @@ fn main() -> Result<(), Box> { t = now.elapsed().unwrap().as_secs_f32(); let target = amplitude * 180.0_f32.to_radians() * (2.0 * PI * 0.5 * t).sin(); - let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), id, MotorValue::{top:target+curr_pos.top, middle:target+curr_pos.middle, bottom:target+curr_pos.bottom}); - match feedback { - Ok(feedback) => { - nbok+=1; - println!("42 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); - }, - Err(e) => { - nberr+=1; - println!("error: {:?}", e); - } - } - - // thread::sleep(Duration::from_micros(500)); + let feedback = orbita3d_poulpe::write_target_position( + &io, + serial_port.as_mut(), + id, + MotorValue:: { + top: target + curr_pos.top, + middle: target + curr_pos.middle, + bottom: target + curr_pos.bottom, + }, + ); + match feedback { + Ok(feedback) => { + nbok += 1; + println!( + "42 target: {} feedback pos: {} {} {}", + target, + feedback.position.top, + feedback.position.middle, + feedback.position.bottom, + ); + } + Err(e) => { + nberr += 1; + println!("error: {:?}", e); + } + } + + // thread::sleep(Duration::from_micros(500)); // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 43, MotorValue::{top:0.0, middle:0.0, bottom:0.0}); // // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 43, MotorValue::{top:0.0, middle:0.0, bottom:0.0}).unwrap_or_else(MotorPositionSpeedLoad::{position:MotorValue::{top:0.0, middle:0.0, bottom:0.0}, speed:MotorValue::{top:0.0, middle:0.0, bottom:0.0}, load:MotorValue::{top:0.0, middle:0.0, bottom:0.0}}); - // match feedback { - // Ok(feedback) => { - // nbok+=1; - // println!("43 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); - // }, - // Err(e) => { - // nberr+=1; - // println!("error: {:?}", e); - // } - // } - - + // match feedback { + // Ok(feedback) => { + // nbok+=1; + // println!("43 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); + // }, + // Err(e) => { + // nberr+=1; + // println!("error: {:?}", e); + // } + // } // println!("43 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); - // thread::sleep(Duration::from_micros(500)); + // thread::sleep(Duration::from_micros(500)); // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 44, MotorValue::{top:0.0, middle:0.0, bottom:0.0}); // // let feedback = orbita3d_poulpe::write_target_position(&io, serial_port.as_mut(), 44, MotorValue::{top:0.0, middle:0.0, bottom:0.0}).unwrap_or_else(MotorPositionSpeedLoad::{position:MotorValue::{top:0.0, middle:0.0, bottom:0.0}, speed:MotorValue::{top:0.0, middle:0.0, bottom:0.0}, load:MotorValue::{top:0.0, middle:0.0, bottom:0.0}}); - // match feedback { - // Ok(feedback) => { - // nbok+=1; - // println!("44 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); - // }, - // Err(e) => { - // nberr+=1; - // println!("error: {:?}", e); - // } - // } - - // nbtot+=3; - nbtot+=1; + // match feedback { + // Ok(feedback) => { + // nbok+=1; + // println!("44 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); + // }, + // Err(e) => { + // nberr+=1; + // println!("error: {:?}", e); + // } + // } + + // nbtot+=3; + nbtot += 1; // println!("44 target: {} feedback pos: {} {} {} feedback vel: {} {} {} feedback torque: {} {} {} ", target, feedback.position.top,feedback.position.middle,feedback.position.bottom,feedback.speed.top,feedback.speed.middle,feedback.speed.bottom,feedback.load.top,feedback.load.middle,feedback.load.bottom); - // let axis_sensor = orbita3d_poulpe::read_axis_sensor(&io, serial_port.as_mut(), id)?; - // println!("axis_sensor: {:6.2} {:6.2} {:6.2}", axis_sensor.top, axis_sensor.middle, axis_sensor.bottom); + // let axis_sensor = orbita3d_poulpe::read_axis_sensor(&io, serial_port.as_mut(), id)?; + // println!("axis_sensor: {:6.2} {:6.2} {:6.2}", axis_sensor.top, axis_sensor.middle, axis_sensor.bottom); - - - - // thread::sleep(Duration::from_micros(1000-t0.elapsed().as_micros() as u64)); + // 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()); + 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!( + "nberr: {} nbtot: {} nbok: {} ratio: {:?}", + nberr, + nbtot, + nbok, + nbok as f32 / nbtot as f32 + ); println!("STOP"); - let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::{top:false, middle:false, bottom:false})?; - + let _ = orbita3d_poulpe::write_torque_enable( + &io, + serial_port.as_mut(), + id, + MotorValue:: { + top: false, + middle: false, + bottom: false, + }, + )?; thread::sleep(Duration::from_millis(2000)); diff --git a/src/protocol/mod.rs b/src/protocol/mod.rs index 60f28b6..f5ef071 100644 --- a/src/protocol/mod.rs +++ b/src/protocol/mod.rs @@ -1,9 +1,11 @@ use serialport::SerialPort; mod v1; +#[allow(unused_imports)] pub use v1::{PacketV1, V1}; mod v2; +#[allow(unused_imports)] pub use v2::{PacketV2, V2}; use crate::{ @@ -32,9 +34,16 @@ pub trait Protocol { self.read_status_packet(port, id).map(|_| ()) } - fn write_fb(&self, port: &mut dyn SerialPort, id: u8, addr: u8, data: &[u8]) -> Result> { + fn write_fb( + &self, + port: &mut dyn SerialPort, + id: u8, + addr: u8, + data: &[u8], + ) -> Result> { self.send_instruction_packet(port, P::write_packet(id, addr, data).as_ref())?; - self.read_status_packet(port, id).map(|sp| sp.params().to_vec()) + self.read_status_packet(port, id) + .map(|sp| sp.params().to_vec()) } fn sync_read( From 02e97c34917c0ffe7832ccb66ec5907bfded28f0 Mon Sep 17 00:00:00 2001 From: Pierre Rouanet Date: Wed, 21 Feb 2024 15:48:43 +0100 Subject: [PATCH 35/52] Fmt. --- src/device/mod.rs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/device/mod.rs b/src/device/mod.rs index dee567f..c4c04fb 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -116,7 +116,6 @@ macro_rules! reg_write_only_fb { }; } - /// Generates read, sync_read, write and sync_write functions for given register #[macro_export] macro_rules! reg_read_write { @@ -134,12 +133,11 @@ macro_rules! reg_read_write_fb { }; } - pub mod l0_force_fan; pub mod mx; pub mod orbita2d_poulpe; pub mod orbita2dof_foc; pub mod orbita_foc; -pub mod xl320; pub mod orbita3d_poulpe; +pub mod xl320; From 02a9b9f7ed7d528d441e1c2e55603b2fb275ef9d Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Thu, 7 Mar 2024 16:09:01 +0100 Subject: [PATCH 36/52] add board_state, add limit_max --- src/device/orbita2d_poulpe.rs | 4 ++++ src/device/orbita3d_poulpe.rs | 4 +++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/device/orbita2d_poulpe.rs b/src/device/orbita2d_poulpe.rs index 8de6720..07a32ef 100644 --- a/src/device/orbita2d_poulpe.rs +++ b/src/device/orbita2d_poulpe.rs @@ -25,7 +25,9 @@ reg_read_only!(firmware_version, 6, u8); reg_read_write!(id, 7, u8); reg_read_write!(velocity_limit, 10, MotorValue::); +reg_read_write!(velocity_limit_max, 12, MotorValue::); reg_read_write!(torque_flux_limit, 14, MotorValue::); +reg_read_write!(torque_flux_limit_max, 16, MotorValue::); reg_read_write!(uq_ud_limit, 18, MotorValue::); reg_read_write!(flux_pid, 20, MotorValue::); @@ -46,6 +48,8 @@ reg_read_write_fb!( MotorPositionSpeedLoad ); +reg_read_write!(board_state, 80, u8); + reg_read_only!(axis_sensor, 90, MotorValue::); reg_read_only!(full_state, 100, MotorPositionSpeedLoad); diff --git a/src/device/orbita3d_poulpe.rs b/src/device/orbita3d_poulpe.rs index 1bb3d50..0bbfae4 100644 --- a/src/device/orbita3d_poulpe.rs +++ b/src/device/orbita3d_poulpe.rs @@ -37,7 +37,9 @@ reg_read_only!(firmware_version, 6, u8); reg_read_write!(id, 7, u8); reg_read_write!(velocity_limit, 10, MotorValue::); +reg_read_write!(velocity_limit_max, 12, MotorValue::); reg_read_write!(torque_flux_limit, 14, MotorValue::); +reg_read_write!(torque_flux_limit_max, 16, MotorValue::); reg_read_write!(uq_ud_limit, 18, MotorValue::); reg_read_write!(flux_pid, 20, MotorValue::); @@ -57,7 +59,7 @@ reg_read_write_fb!( MotorValue::, MotorPositionSpeedLoad ); - +reg_read_write!(board_state, 80, u8); reg_read_only!(axis_sensor, 90, MotorValue::); reg_read_only!(index_sensor, 99, MotorValue::); reg_read_only!(full_state, 100, MotorPositionSpeedLoad); From a9d91fc8cf42d1accdcbfbd5311f79ce569e7d0e Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Tue, 30 Apr 2024 11:39:32 +0200 Subject: [PATCH 37/52] Adding xm device --- src/device/mod.rs | 1 + src/device/xm.rs | 62 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 63 insertions(+) create mode 100644 src/device/xm.rs diff --git a/src/device/mod.rs b/src/device/mod.rs index cc2f346..1187d35 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -92,3 +92,4 @@ pub mod mx; pub mod orbita2dof_foc; pub mod orbita_foc; pub mod xl320; +pub mod xm; diff --git a/src/device/xm.rs b/src/device/xm.rs new file mode 100644 index 0000000..1d3da68 --- /dev/null +++ b/src/device/xm.rs @@ -0,0 +1,62 @@ +//! MX robotis register (protocol v1) +//! +//! Despite some minor differences among MX variants, it should work for +//! * MX-28 +//! * MX-64 +//! * MX-106 +//! +//! See for example. + +use crate::device::*; + +reg_read_only!(model_number, 0, u16); +reg_read_only!(firmware_version, 6, u8); +reg_read_write!(id, 7, u8); +reg_read_write!(baud_rate, 8, u8); +reg_read_write!(return_delay_time, 9, u8); +reg_read_write!(drive_mode, 10, u8); +reg_read_write!(operating_mode, 11, u8); +reg_read_write!(secondary_shadow_id, 12, u8); +reg_read_write!(protocol_type, 13, u8); +reg_read_write!(homing_offset, 20, i32); +reg_read_write!(moving_threshold, 24, u32); +reg_read_write!(temperature_limit, 31, u8); +reg_read_write!(max_voltage_limit, 32, u16); +reg_read_write!(min_voltage_limit, 34, u16); +reg_read_write!(acceleration_limit, 40, u32); +reg_read_write!(torque_limit, 48, u16); +reg_read_write!(velocity_limit, 44, u32); +reg_read_write!(max_position_limit, 48, u32); +reg_read_write!(min_position_limit, 52, u32); + +reg_read_write!(shutdown, 63, u8); +reg_read_write!(torque_enable, 64, u8); +reg_read_write!(led, 65, u8); +reg_read_write!(status_return_level, 68, u8); +reg_read_write!(registered_instruction, 69, u8); +reg_read_write!(hardware_error_status, 70, u8); +reg_read_write!(velocity_i_gain, 76, u16); +reg_read_write!(velocity_p_gain, 78, u16); +reg_read_write!(position_d_gain, 80, u16); +reg_read_write!(position_i_gain, 82, u16); +reg_read_write!(position_p_gain, 84, u16); +reg_read_write!(feedforward_2nd_gain, 88, u16); +reg_read_write!(feedforward_1st_gain, 90, u16); +reg_read_write!(bus_watchdog, 98, u8); + +reg_read_write!(goal_pwm, 100, u16); +reg_read_write!(goal_velocity, 104, u32); +reg_read_write!(profile_acceleration, 108, u32); +reg_read_write!(profile_velocity, 112, u32); +reg_read_write!(goal_position, 116, u32); +reg_read_only!(realtime_tick, 120, u16); +reg_read_only!(moving, 122, u8); +reg_read_only!(moving_status, 123, u8); +reg_read_only!(present_pwm, 124, u16); +reg_read_only!(present_load, 126, u16); +reg_read_only!(present_velocity, 128, u32); +reg_read_only!(present_position, 132, u32); +reg_read_only!(velocity_trajectory, 136, u32); +reg_read_only!(position_trajectory, 140, u32); +reg_read_only!(present_input_voltage, 144, u16); +reg_read_only!(present_temperature, 146, u8); From 7bfc680d0eea9e10d54bf92429d24ae055c25e81 Mon Sep 17 00:00:00 2001 From: gospar Date: Fri, 3 May 2024 10:25:55 +0200 Subject: [PATCH 38/52] added the check for the lenght of receved data before running try_into().unwrap() - #52 --- src/device/mod.rs | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/src/device/mod.rs b/src/device/mod.rs index c4c04fb..403420b 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -18,6 +18,7 @@ macro_rules! reg_read_only { id: u8, ) -> Result<$reg_type> { let val = io.read(serial_port, id, $addr, size_of::<$reg_type>().try_into().unwrap())?; + check_response_size!(&val, $reg_type); let val = $reg_type::from_le_bytes(val.try_into().unwrap()); Ok(val) @@ -30,6 +31,7 @@ macro_rules! reg_read_only { ids: &[u8], ) -> Result> { let val = io.sync_read(serial_port, ids, $addr, size_of::<$reg_type>().try_into().unwrap())?; + check_response_size!(&val, $reg_type); let val = val .iter() .map(|v| $reg_type::from_le_bytes(v.as_slice().try_into().unwrap())) @@ -91,8 +93,9 @@ macro_rules! reg_write_only_fb { val: $reg_type, ) -> Result<$fb_type> { let fb=io.write_fb(serial_port, id, $addr, &val.to_le_bytes())?; - let fb = $fb_type::from_le_bytes(fb.try_into().unwrap()); - Ok(fb) + check_response_size!(&fb, $fb_type); + let fb = $fb_type::from_le_bytes(fb.try_into().unwrap()); + Ok(fb) } #[doc = concat!("Sync write register *", stringify!($name), "* (addr: ", stringify!($addr), ", type: ", stringify!($reg_type), ")")] @@ -133,6 +136,21 @@ macro_rules! reg_read_write_fb { }; } +// Check if the response size is correct +// If not, return an error +// response is a Vec +macro_rules! check_response_size { + ($response:expr, $reg_type:ty) => { + { + let response = $response; + if response.len() != std::mem::size_of::<$reg_type>() { + let message = format!("Invalid response size, expected {} received {}", std::mem::size_of::<$reg_type>(), response.len()); + return Err(message.into()); + } + } + }; +} + pub mod l0_force_fan; pub mod mx; pub mod orbita2d_poulpe; From 8841fc2ba6bf5c06f773395ba0ff4b323fdafdc6 Mon Sep 17 00:00:00 2001 From: gospar Date: Fri, 3 May 2024 10:30:07 +0200 Subject: [PATCH 39/52] a nice formating --- src/device/mod.rs | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/device/mod.rs b/src/device/mod.rs index 403420b..cb58384 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -140,15 +140,17 @@ macro_rules! reg_read_write_fb { // If not, return an error // response is a Vec macro_rules! check_response_size { - ($response:expr, $reg_type:ty) => { - { - let response = $response; - if response.len() != std::mem::size_of::<$reg_type>() { - let message = format!("Invalid response size, expected {} received {}", std::mem::size_of::<$reg_type>(), response.len()); - return Err(message.into()); - } + ($response:expr, $reg_type:ty) => {{ + let response = $response; + if response.len() != std::mem::size_of::<$reg_type>() { + let message = format!( + "Invalid response size, expected {} received {}", + std::mem::size_of::<$reg_type>(), + response.len() + ); + return Err(message.into()); } - }; + }}; } pub mod l0_force_fan; From b9e1769386fb9a72107c3e08dd93c19eb0d1b7c7 Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Tue, 14 May 2024 13:58:45 +0200 Subject: [PATCH 40/52] Adding xl330 and xl430 support with chatgpt https://chat.openai.com/share/1840d491-8942-4bc9-8593-17915677882d --- src/device/xl330.rs | 75 +++++++++++++++++++++++++++++++++++++++++++++ src/device/xl430.rs | 74 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 149 insertions(+) create mode 100644 src/device/xl330.rs create mode 100644 src/device/xl430.rs diff --git a/src/device/xl330.rs b/src/device/xl330.rs new file mode 100644 index 0000000..c79c43c --- /dev/null +++ b/src/device/xl330.rs @@ -0,0 +1,75 @@ +//! XL-330 robotis register (protocol v2) +//! +//! See for details. + +use crate::device::*; + +reg_read_only!(model_number, 0, u16); +reg_read_only!(model_information, 2, u32); +reg_read_write!(firmware_version, 6, u8); +reg_read_write!(id, 7, u8); +reg_read_write!(baud_rate, 8, u8); +reg_read_write!(return_delay_time, 9, u8); +reg_read_write!(drive_mode, 10, u8); +reg_read_write!(operating_mode, 11, u8); +reg_read_write!(secondary_id, 12, u8); +reg_read_write!(protocol_type, 13, u8); +reg_read_write!(homing_offset, 20, i32); +reg_read_write!(moving_threshold, 24, u32); +reg_read_write!(temperature_limit, 31, u8); +reg_read_write!(max_voltage_limit, 32, u16); +reg_read_write!(min_voltage_limit, 34, u16); +reg_read_write!(pwm_limit, 36, u16); +reg_read_write!(current_limit, 38, u16); +reg_read_write!(acceleration_limit, 40, u32); +reg_read_write!(velocity_limit, 44, u32); +reg_read_write!(max_position_limit, 48, u32); +reg_read_write!(min_position_limit, 52, u32); +reg_read_write!(startup_configuration, 60, u8); +reg_read_write!(pwm_slope, 62, u8); +reg_read_write!(shutdown, 63, u8); + +reg_read_write!(torque_enable, 64, u8); +reg_read_write!(led, 65, u8); +reg_read_write!(status_return_level, 68, u8); +reg_read_write!(registered_instruction, 69, u8); +reg_read_write!(hardware_error_status, 70, u8); +reg_read_write!(velocity_i_gain, 76, u16); +reg_read_write!(velocity_p_gain, 78, u16); +reg_read_write!(position_d_gain, 80, u16); +reg_read_write!(position_i_gain, 82, u16); +reg_read_write!(position_p_gain, 84, u16); +reg_read_write!(feedforward_2nd_gain, 88, u16); +reg_read_write!(feedforward_1st_gain, 90, u16); +reg_read_write!(bus_watchdog, 98, u8); +reg_read_write!(goal_pwm, 100, u16); +reg_read_write!(goal_current, 102, u16); +reg_read_write!(goal_velocity, 104, u32); +reg_read_write!(profile_acceleration, 108, u32); +reg_read_write!(profile_velocity, 112, u32); +reg_read_write!(goal_position, 116, u32); +reg_read_only!(realtime_tick, 120, u16); +reg_read_only!(moving, 122, u8); +reg_read_only!(moving_status, 123, u8); +reg_read_only!(present_pwm, 124, u16); +reg_read_only!(present_current, 126, u16); +reg_read_only!(present_velocity, 128, u32); +reg_read_only!(present_position, 132, u32); +reg_read_only!(velocity_trajectory, 136, u32); +reg_read_only!(position_trajectory, 140, u32); +reg_read_only!(present_input_voltage, 144, u16); +reg_read_only!(present_temperature, 146, u8); +reg_read_only!(backup_ready, 147, u8); + +reg_read_write!(indirect_address_1, 168, u16); +reg_read_write!(indirect_address_2, 170, u16); +reg_read_write!(indirect_address_3, 172, u16); +reg_read_write!(indirect_address_4, 174, u16); +reg_read_write!(indirect_address_5, 176, u16); +reg_read_write!(indirect_address_6, 178, u16); +reg_read_write!(indirect_data_1, 224, u8); +reg_read_write!(indirect_data_2, 225, u8); +reg_read_write!(indirect_data_3, 226, u8); +reg_read_write!(indirect_data_4, 227, u8); +reg_read_write!(indirect_data_5, 228, u8); +reg_read_write!(indirect_data_6, 229, u8); diff --git a/src/device/xl430.rs b/src/device/xl430.rs new file mode 100644 index 0000000..dee3d7f --- /dev/null +++ b/src/device/xl430.rs @@ -0,0 +1,74 @@ +//! XL-430 robotis register (protocol v2) +//! +//! See for details. + +use crate::device::*; + +reg_read_only!(model_number, 0, u16); +reg_read_only!(model_information, 2, u32); +reg_read_write!(firmware_version, 6, u8); +reg_read_write!(id, 7, u8); +reg_read_write!(baud_rate, 8, u8); +reg_read_write!(return_delay_time, 9, u8); +reg_read_write!(drive_mode, 10, u8); +reg_read_write!(operating_mode, 11, u8); +reg_read_write!(secondary_id, 12, u8); +reg_read_write!(protocol_type, 13, u8); +reg_read_write!(homing_offset, 20, i32); +reg_read_write!(moving_threshold, 24, u32); +reg_read_write!(temperature_limit, 31, u8); +reg_read_write!(max_voltage_limit, 32, u16); +reg_read_write!(min_voltage_limit, 34, u16); +reg_read_write!(pwm_limit, 36, u16); +reg_read_write!(current_limit, 38, u16); +reg_read_write!(acceleration_limit, 40, u32); +reg_read_write!(velocity_limit, 44, u32); +reg_read_write!(max_position_limit, 48, u32); +reg_read_write!(min_position_limit, 52, u32); +reg_read_write!(startup_configuration, 60, u8); +reg_read_write!(shutdown, 63, u8); + +reg_read_write!(torque_enable, 64, u8); +reg_read_write!(led, 65, u8); +reg_read_write!(status_return_level, 68, u8); +reg_read_write!(registered_instruction, 69, u8); +reg_read_write!(hardware_error_status, 70, u8); +reg_read_write!(velocity_i_gain, 76, u16); +reg_read_write!(velocity_p_gain, 78, u16); +reg_read_write!(position_d_gain, 80, u16); +reg_read_write!(position_i_gain, 82, u16); +reg_read_write!(position_p_gain, 84, u16); +reg_read_write!(feedforward_2nd_gain, 88, u16); +reg_read_write!(feedforward_1st_gain, 90, u16); +reg_read_write!(bus_watchdog, 98, u8); +reg_read_write!(goal_pwm, 100, u16); +reg_read_write!(goal_current, 102, u16); +reg_read_write!(goal_velocity, 104, u32); +reg_read_write!(profile_acceleration, 108, u32); +reg_read_write!(profile_velocity, 112, u32); +reg_read_write!(goal_position, 116, u32); +reg_read_only!(realtime_tick, 120, u16); +reg_read_only!(moving, 122, u8); +reg_read_only!(moving_status, 123, u8); +reg_read_only!(present_pwm, 124, u16); +reg_read_only!(present_current, 126, u16); +reg_read_only!(present_velocity, 128, u32); +reg_read_only!(present_position, 132, u32); +reg_read_only!(velocity_trajectory, 136, u32); +reg_read_only!(position_trajectory, 140, u32); +reg_read_only!(present_input_voltage, 144, u16); +reg_read_only!(present_temperature, 146, u8); +reg_read_only!(backup_ready, 147, u8); + +reg_read_write!(indirect_address_1, 168, u16); +reg_read_write!(indirect_address_2, 170, u16); +reg_read_write!(indirect_address_3, 172, u16); +reg_read_write!(indirect_address_4, 174, u16); +reg_read_write!(indirect_address_5, 176, u16); +reg_read_write!(indirect_address_6, 178, u16); +reg_read_write!(indirect_data_1, 224, u8); +reg_read_write!(indirect_data_2, 225, u8); +reg_read_write!(indirect_data_3, 226, u8); +reg_read_write!(indirect_data_4, 227, u8); +reg_read_write!(indirect_data_5, 228, u8); +reg_read_write!(indirect_data_6, 229, u8); From 3b2930da529c2ed1f9e590e6cba84d06f3a40d0b Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Tue, 14 May 2024 14:10:14 +0200 Subject: [PATCH 41/52] Add device in mod --- src/device/mod.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/device/mod.rs b/src/device/mod.rs index 1187d35..d5a24e8 100644 --- a/src/device/mod.rs +++ b/src/device/mod.rs @@ -92,4 +92,6 @@ pub mod mx; pub mod orbita2dof_foc; pub mod orbita_foc; pub mod xl320; +pub mod xl330; +pub mod xl430; pub mod xm; From 0a7a8e3b5a0aaed8f0f04f3083482f17224167db Mon Sep 17 00:00:00 2001 From: Augustin Crampette Date: Fri, 24 May 2024 17:10:36 +0200 Subject: [PATCH 42/52] Add current based position registers --- src/device/xm.rs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/device/xm.rs b/src/device/xm.rs index 1d3da68..08a5bfa 100644 --- a/src/device/xm.rs +++ b/src/device/xm.rs @@ -24,7 +24,7 @@ reg_read_write!(temperature_limit, 31, u8); reg_read_write!(max_voltage_limit, 32, u16); reg_read_write!(min_voltage_limit, 34, u16); reg_read_write!(acceleration_limit, 40, u32); -reg_read_write!(torque_limit, 48, u16); +reg_read_write!(current_limit, 38, u16); reg_read_write!(velocity_limit, 44, u32); reg_read_write!(max_position_limit, 48, u32); reg_read_write!(min_position_limit, 52, u32); @@ -45,6 +45,7 @@ reg_read_write!(feedforward_1st_gain, 90, u16); reg_read_write!(bus_watchdog, 98, u8); reg_read_write!(goal_pwm, 100, u16); +reg_read_write!(goal_current, 102, u16); reg_read_write!(goal_velocity, 104, u32); reg_read_write!(profile_acceleration, 108, u32); reg_read_write!(profile_velocity, 112, u32); @@ -53,7 +54,7 @@ reg_read_only!(realtime_tick, 120, u16); reg_read_only!(moving, 122, u8); reg_read_only!(moving_status, 123, u8); reg_read_only!(present_pwm, 124, u16); -reg_read_only!(present_load, 126, u16); +reg_read_only!(present_current, 126, u16); reg_read_only!(present_velocity, 128, u32); reg_read_only!(present_position, 132, u32); reg_read_only!(velocity_trajectory, 136, u32); From 8c89e556501c7fb29066df06772ce7ab820a84c4 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 4 Jun 2024 16:54:43 +0200 Subject: [PATCH 43/52] add conversion --- src/device/xm.rs | 107 ++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 101 insertions(+), 6 deletions(-) diff --git a/src/device/xm.rs b/src/device/xm.rs index 08a5bfa..edb6e4e 100644 --- a/src/device/xm.rs +++ b/src/device/xm.rs @@ -1,11 +1,8 @@ -//! MX robotis register (protocol v1) +//! XM robotis register (protocol v1) //! -//! Despite some minor differences among MX variants, it should work for -//! * MX-28 -//! * MX-64 -//! * MX-106 +//! Tested for the XM430-W210 flashed in protocol v1 //! -//! See for example. +//! See for example. use crate::device::*; @@ -25,7 +22,9 @@ reg_read_write!(max_voltage_limit, 32, u16); reg_read_write!(min_voltage_limit, 34, u16); reg_read_write!(acceleration_limit, 40, u32); reg_read_write!(current_limit, 38, u16); +reg_read_write!(torque_limit, 38, u16); //Duplicate with MX name for compatibility reg_read_write!(velocity_limit, 44, u32); +reg_read_write!(moving_speed, 44, u32); //Duplicate with MX name for compatibility reg_read_write!(max_position_limit, 48, u32); reg_read_write!(min_position_limit, 52, u32); @@ -61,3 +60,99 @@ reg_read_only!(velocity_trajectory, 136, u32); reg_read_only!(position_trajectory, 140, u32); reg_read_only!(present_input_voltage, 144, u16); reg_read_only!(present_temperature, 146, u8); + + + +/// Unit conversion for XM motors +pub mod conv { + use std::f64::consts::PI; + + /// Dynamixel angular position to radians + /// + /// Works in joint and multi-turn mode + pub fn dxl_pos_to_radians(pos: i32) -> f64 { + (2.0 * PI * (pos as f64) / 4096.0) - PI + } + /// Radians to dynamixel angular position + /// + /// Works in joint and multi-turn mode + pub fn radians_to_dxl_pos(rads: f64) -> i32 { + (4096.0 * (PI + rads) / (2.0 * PI)) as i16 + } + + /// Dynamixel absolute speed to radians per second + /// + /// Works for moving_speed in joint mode for instance + pub fn dxl_abs_speed_to_rad_per_sec(speed: u32) -> f64 { + let rpm = speed as f64 * 0.229; + rpm * 0.10472 + } + /// Radians per second to dynamixel absolute speed + /// + /// Works for moving_speed in joint mode for instance + pub fn rad_per_sec_to_dxl_abs_speed(speed: f64) -> u32 { + let rpm = speed / 0.10472; + (rpm / 0.229) as u16 + } + /// Dynamixel speed to radians per second + /// + /// Works for present_speed for instance + pub fn dxl_oriented_speed_to_rad_per_sec(speed: u32) -> f64 { + let cw = (speed >> 11) == 1; + + let rad_per_sec = dxl_abs_speed_to_rad_per_sec(speed % 1024); + + match cw { + true => rad_per_sec, + false => -rad_per_sec, + } + } + /// Radians per second to dynamixel speed + /// + /// Works for present_speed for instance + pub fn rad_per_sec_to_dxl_oriented_speed(speed: f64) -> u32 { + let raw = rad_per_sec_to_dxl_abs_speed(speed.abs()); + + match speed < 0.0 { + true => raw, + false => raw + 2048, + } + } + + /// Dynamixel absolute load to torque percentage + /// + /// Works for torque_limit for instance + pub fn dxl_load_to_abs_torque(load: u16) -> f64 { + load as f64 / 1193.0 * 100.0 + } + /// Torque percentage to dynamixel absolute load + /// + /// Works for torque_limit for instance + pub fn torque_to_dxl_abs_load(torque: f64) -> u16 { + assert!((0.0..=100.0).contains(&torque)); + + (torque * 1193.0 / 100.0) as u16 + } + /// Dynamixel load to torque percentage + /// + /// Works for present_torque for instance + pub fn dxl_load_to_oriented_torque(load: u16) -> f64 { + let cw = (load >> 10) == 1; + + let torque = dxl_load_to_abs_torque(load % 1193); + + match cw { + true => torque, + false => -torque, + } + } + /// Torque percentage to dynamixel load + pub fn oriented_torque_to_dxl_load(torque: f64) -> u16 { + let load = torque_to_dxl_abs_load(torque.abs()); + + match torque < 0.0 { + true => load, + false => load + 1193, + } + } +} From 6fdc161b46bd1cd8b065a0b0886c5b62c26a94ae Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 4 Jun 2024 16:56:15 +0200 Subject: [PATCH 44/52] type mismatch --- src/device/xm.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/device/xm.rs b/src/device/xm.rs index edb6e4e..f97ecdb 100644 --- a/src/device/xm.rs +++ b/src/device/xm.rs @@ -77,7 +77,7 @@ pub mod conv { /// /// Works in joint and multi-turn mode pub fn radians_to_dxl_pos(rads: f64) -> i32 { - (4096.0 * (PI + rads) / (2.0 * PI)) as i16 + (4096.0 * (PI + rads) / (2.0 * PI)) as i32 } /// Dynamixel absolute speed to radians per second @@ -92,7 +92,7 @@ pub mod conv { /// Works for moving_speed in joint mode for instance pub fn rad_per_sec_to_dxl_abs_speed(speed: f64) -> u32 { let rpm = speed / 0.10472; - (rpm / 0.229) as u16 + (rpm / 0.229) as u32 } /// Dynamixel speed to radians per second /// From 012062f1bf31ca1f1ac52fe877314ff7968a9861 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 4 Jun 2024 17:00:52 +0200 Subject: [PATCH 45/52] type mismatch --- src/device/xm.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/device/xm.rs b/src/device/xm.rs index f97ecdb..97ac58b 100644 --- a/src/device/xm.rs +++ b/src/device/xm.rs @@ -54,8 +54,8 @@ reg_read_only!(moving, 122, u8); reg_read_only!(moving_status, 123, u8); reg_read_only!(present_pwm, 124, u16); reg_read_only!(present_current, 126, u16); -reg_read_only!(present_velocity, 128, u32); -reg_read_only!(present_position, 132, u32); +reg_read_only!(present_velocity, 128, i32); +reg_read_only!(present_position, 132, i32); reg_read_only!(velocity_trajectory, 136, u32); reg_read_only!(position_trajectory, 140, u32); reg_read_only!(present_input_voltage, 144, u16); From 424861fea4aca60d50e495bfa374cebc168f29ad Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 4 Jun 2024 17:01:39 +0200 Subject: [PATCH 46/52] type mismatch --- src/device/xm.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/device/xm.rs b/src/device/xm.rs index 97ac58b..d37fc53 100644 --- a/src/device/xm.rs +++ b/src/device/xm.rs @@ -45,10 +45,10 @@ reg_read_write!(bus_watchdog, 98, u8); reg_read_write!(goal_pwm, 100, u16); reg_read_write!(goal_current, 102, u16); -reg_read_write!(goal_velocity, 104, u32); +reg_read_write!(goal_velocity, 104, i32); reg_read_write!(profile_acceleration, 108, u32); reg_read_write!(profile_velocity, 112, u32); -reg_read_write!(goal_position, 116, u32); +reg_read_write!(goal_position, 116, i32); reg_read_only!(realtime_tick, 120, u16); reg_read_only!(moving, 122, u8); reg_read_only!(moving_status, 123, u8); From d82bf59dd87db3d05f88b329da6bfb490635c72a Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 4 Jun 2024 17:07:28 +0200 Subject: [PATCH 47/52] add the post delay --- src/lib.rs | 42 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/src/lib.rs b/src/lib.rs index 73309aa..95980a6 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -28,6 +28,8 @@ //! ``` mod protocol; +use std::time::Duration; + pub use protocol::CommunicationErrorKind; use protocol::{Protocol, V1, V2}; @@ -48,6 +50,7 @@ enum Protocols { /// Raw dynamixel communication messages controller (protocol v1 or v2) pub struct DynamixelSerialIO { protocol: Protocols, + post_delay: Option, } impl DynamixelSerialIO { @@ -74,6 +77,7 @@ impl DynamixelSerialIO { pub fn v1() -> Self { DynamixelSerialIO { protocol: Protocols::V1(V1), + post_delay: None, } } /// Creates a protocol v2 communication IO. @@ -99,6 +103,15 @@ impl DynamixelSerialIO { pub fn v2() -> Self { DynamixelSerialIO { protocol: Protocols::V2(V2), + post_delay: None, + } + } + + /// Set a delay after each communication. + pub fn with_post_delay(self, delay: Duration) -> Self { + DynamixelSerialIO { + post_delay: Some(delay), + ..self } } @@ -172,10 +185,14 @@ impl DynamixelSerialIO { addr: u8, length: u8, ) -> Result> { - match &self.protocol { + let res = match &self.protocol { Protocols::V1(p) => p.read(serial_port, id, addr, length), Protocols::V2(p) => p.read(serial_port, id, addr, length), + }; + if let Some(delay) = self.post_delay { + std::thread::sleep(delay); } + res } /// Writes raw bytes to register. @@ -217,6 +234,29 @@ impl DynamixelSerialIO { match &self.protocol { Protocols::V1(p) => p.write(serial_port, id, addr, data), Protocols::V2(p) => p.write(serial_port, id, addr, data), + }?; + if let Some(delay) = self.post_delay { + std::thread::sleep(delay); + } + Ok(()) + } + + pub fn write_fb( + &self, + serial_port: &mut dyn serialport::SerialPort, + id: u8, + addr: u8, + data: &[u8], + ) -> Result> { + match &self.protocol { + Protocols::V1(p) => { + let res = p.write_fb(serial_port, id, addr, data); + if let Some(delay) = self.post_delay { + std::thread::sleep(delay); + } + res + } + Protocols::V2(_) => Err(Box::new(CommunicationErrorKind::Unsupported)), } } From 5f482c2c0c75dbe22c7fbd3a938de6bc2f50d485 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 4 Jun 2024 17:09:36 +0200 Subject: [PATCH 48/52] add unsupported error kind --- src/protocol/mod.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/protocol/mod.rs b/src/protocol/mod.rs index 2c194b8..3ea36ad 100644 --- a/src/protocol/mod.rs +++ b/src/protocol/mod.rs @@ -130,6 +130,7 @@ impl fmt::Display for CommunicationErrorKind { CommunicationErrorKind::IncorrectId(sender_id, resp_id) => { write!(f, "Incorrect id ({} instead of {})", resp_id, sender_id) } + CommunicationErrorKind::Unsupported => write!(f, "Operation not supported"), } } } From 1d6e9ccc00ae510315baddce62d22629db63dd45 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 4 Jun 2024 17:10:31 +0200 Subject: [PATCH 49/52] add unsupported error kind... --- src/protocol/mod.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/protocol/mod.rs b/src/protocol/mod.rs index 3ea36ad..d3e6741 100644 --- a/src/protocol/mod.rs +++ b/src/protocol/mod.rs @@ -120,6 +120,7 @@ pub enum CommunicationErrorKind { TimeoutError, /// Incorrect response id - different from sender (sender id, response id) IncorrectId(u8, u8), + Unsupported, } impl fmt::Display for CommunicationErrorKind { fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { From 7eadb18dbedff4a78841000afa8d600c19c67289 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Tue, 4 Jun 2024 17:14:21 +0200 Subject: [PATCH 50/52] add write_fb --- src/protocol/mod.rs | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/protocol/mod.rs b/src/protocol/mod.rs index d3e6741..7d10b1e 100644 --- a/src/protocol/mod.rs +++ b/src/protocol/mod.rs @@ -31,6 +31,17 @@ pub trait Protocol { self.send_instruction_packet(port, P::write_packet(id, addr, data).as_ref())?; self.read_status_packet(port, id).map(|_| ()) } + fn write_fb( + &self, + port: &mut dyn SerialPort, + id: u8, + addr: u8, + data: &[u8], + ) -> Result> { + self.send_instruction_packet(port, P::write_packet(id, addr, data).as_ref())?; + self.read_status_packet(port, id) + .map(|sp| sp.params().to_vec()) + } fn sync_read( &self, port: &mut dyn SerialPort, From 51218eebb0f3224100f2e1a91f24647ff001fc3c Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Thu, 26 Sep 2024 11:52:14 +0200 Subject: [PATCH 51/52] update dxl xm device --- Cargo.toml | 1 + src/device/xm.rs | 123 +++++++++++++++++++++++------------------------ 2 files changed, 60 insertions(+), 64 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 7292d7d..010c19a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -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 diff --git a/src/device/xm.rs b/src/device/xm.rs index d37fc53..7b08dcb 100644 --- a/src/device/xm.rs +++ b/src/device/xm.rs @@ -20,6 +20,7 @@ reg_read_write!(moving_threshold, 24, u32); reg_read_write!(temperature_limit, 31, u8); reg_read_write!(max_voltage_limit, 32, u16); reg_read_write!(min_voltage_limit, 34, u16); +reg_read_write!(pwm_limit, 36, u16); reg_read_write!(acceleration_limit, 40, u32); reg_read_write!(current_limit, 38, u16); reg_read_write!(torque_limit, 38, u16); //Duplicate with MX name for compatibility @@ -44,7 +45,7 @@ reg_read_write!(feedforward_1st_gain, 90, u16); reg_read_write!(bus_watchdog, 98, u8); reg_read_write!(goal_pwm, 100, u16); -reg_read_write!(goal_current, 102, u16); +reg_read_write!(goal_current, 102, i16); reg_read_write!(goal_velocity, 104, i32); reg_read_write!(profile_acceleration, 108, u32); reg_read_write!(profile_velocity, 112, u32); @@ -53,7 +54,7 @@ reg_read_only!(realtime_tick, 120, u16); reg_read_only!(moving, 122, u8); reg_read_only!(moving_status, 123, u8); reg_read_only!(present_pwm, 124, u16); -reg_read_only!(present_current, 126, u16); +reg_read_only!(present_current, 126, i16); reg_read_only!(present_velocity, 128, i32); reg_read_only!(present_position, 132, i32); reg_read_only!(velocity_trajectory, 136, u32); @@ -61,98 +62,92 @@ reg_read_only!(position_trajectory, 140, u32); reg_read_only!(present_input_voltage, 144, u16); reg_read_only!(present_temperature, 146, u8); - - /// Unit conversion for XM motors pub mod conv { - use std::f64::consts::PI; + use std::f32::consts::PI; /// Dynamixel angular position to radians /// /// Works in joint and multi-turn mode - pub fn dxl_pos_to_radians(pos: i32) -> f64 { - (2.0 * PI * (pos as f64) / 4096.0) - PI + /// 2048->180° is the center position with 0.088 [deg/pulse] + + pub fn dxl_pos_to_radians(pos: i32) -> f32 { + (2.0 * PI * (pos as f32) / 4096.0) - PI } /// Radians to dynamixel angular position /// /// Works in joint and multi-turn mode - pub fn radians_to_dxl_pos(rads: f64) -> i32 { + pub fn radians_to_dxl_pos(rads: f32) -> i32 { (4096.0 * (PI + rads) / (2.0 * PI)) as i32 } - /// Dynamixel absolute speed to radians per second + /// Dynamixel velocity in rpm /// - /// Works for moving_speed in joint mode for instance - pub fn dxl_abs_speed_to_rad_per_sec(speed: u32) -> f64 { - let rpm = speed as f64 * 0.229; - rpm * 0.10472 + /// Works for present_velocity instance + pub fn dxl_vel_to_rpm(vel: i32) -> f32 { + vel as f32 * 0.229 } - /// Radians per second to dynamixel absolute speed + /// Velocity (rpm) to dynamixel velocity /// - /// Works for moving_speed in joint mode for instance - pub fn rad_per_sec_to_dxl_abs_speed(speed: f64) -> u32 { - let rpm = speed / 0.10472; - (rpm / 0.229) as u32 + /// It should be in [-velocity_limit, +velocity_limit] with an absolute max at 1023 (324.267rpm) + /// Works for goal_current for instance + pub fn rpm_to_dxl_vel(rpm: f32) -> i32 { + (rpm / 0.229) as i32 } - /// Dynamixel speed to radians per second - /// - /// Works for present_speed for instance - pub fn dxl_oriented_speed_to_rad_per_sec(speed: u32) -> f64 { - let cw = (speed >> 11) == 1; - let rad_per_sec = dxl_abs_speed_to_rad_per_sec(speed % 1024); - - match cw { - true => rad_per_sec, - false => -rad_per_sec, - } + /// Dynamixel current to mA + /// + /// Works for present_current instance + pub fn dxl_current_to_ma(current: i16) -> f32 { + current as f32 * 2.69 } - /// Radians per second to dynamixel speed + /// Current (mA) to dynamixel current /// - /// Works for present_speed for instance - pub fn rad_per_sec_to_dxl_oriented_speed(speed: f64) -> u32 { - let raw = rad_per_sec_to_dxl_abs_speed(speed.abs()); - - match speed < 0.0 { - true => raw, - false => raw + 2048, - } + /// It should be in [-current_limit, +current_limit] with an absolute max at 1193 (3209.17mA) + /// Works for goal_current for instance + pub fn current_to_dxl_ma(current: f32) -> i16 { + (current / 2.69) as i16 } - /// Dynamixel absolute load to torque percentage + /// Dxl Temperature (°C) /// - /// Works for torque_limit for instance - pub fn dxl_load_to_abs_torque(load: u16) -> f64 { - load as f64 / 1193.0 * 100.0 + /// read_current_temperature + pub fn dxl_to_temperature(temp: u8) -> f32 { + temp as f32 } - /// Torque percentage to dynamixel absolute load - /// - /// Works for torque_limit for instance - pub fn torque_to_dxl_abs_load(torque: f64) -> u16 { - assert!((0.0..=100.0).contains(&torque)); - (torque * 1193.0 / 100.0) as u16 + /// Temperature (°C) to dxl + /// + /// write_temperature_limit + pub fn temperature_to_dxl(temp: f32) -> u8 { + temp as u8 } - /// Dynamixel load to torque percentage + + /// Dynamixel pwm to % /// - /// Works for present_torque for instance - pub fn dxl_load_to_oriented_torque(load: u16) -> f64 { - let cw = (load >> 10) == 1; + /// Works for present_pwm + pub fn dxl_pwm_to_percentage(pwm: u16) -> f32 { + pwm as f32 * 0.113 + } - let torque = dxl_load_to_abs_torque(load % 1193); + /// PWM (%) to dynamixel pwm + /// + /// Works for pwm_limit + pub fn percentage_to_dxl_pwm(pwm: f32) -> u16 { + (pwm / 0.113) as u16 + } - match cw { - true => torque, - false => -torque, - } + /// Dynamixel voltage to V + /// + /// Works for present_voltage + pub fn dxl_to_volt(volt: u16) -> f32 { + volt as f32 * 0.1 } - /// Torque percentage to dynamixel load - pub fn oriented_torque_to_dxl_load(torque: f64) -> u16 { - let load = torque_to_dxl_abs_load(torque.abs()); - match torque < 0.0 { - true => load, - false => load + 1193, - } + /// V to dynamixel voltage + /// + /// Works for voltage_limit + pub fn volt_to_dxl(volt: f32) -> u16 { + (volt / 0.1) as u16 } } From 781de4e9dbb031a305d9faa79f4d5478b5fa65d5 Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Thu, 26 Sep 2024 09:57:50 +0000 Subject: [PATCH 52/52] Update Changelog.md --- Changelog.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Changelog.md b/Changelog.md index f2eae5c..21ebc73 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,7 @@ ## Version 0.5.0 - Add an post delay option to the read and write method. +- Add dxl XM motor device ## Version 0.4.0 @@ -22,4 +23,4 @@ - Support protocol v1 and v2 - Support read, sync read, write, sync write, ping -- Support mx and xl-320 \ No newline at end of file +- Support mx and xl-320