update dxl xm device

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

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,106 +54,100 @@ 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);
 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
     }
 }