From 51218eebb0f3224100f2e1a91f24647ff001fc3c Mon Sep 17 00:00:00 2001 From: Steve Nguyen Date: Thu, 26 Sep 2024 11:52:14 +0200 Subject: [PATCH] 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 } }