limits types

pollen-robotics · Jan 9, 2024 · 45eb741 · 45eb741
1 parent 4abeec6
commit 45eb741
Show file tree

Hide file tree

Showing 4 changed files with 188 additions and 16 deletions.
diff --git 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<dyn Error>> {
     println!("Ping {:?}: {:?}", id,x);
     thread::sleep(Duration::from_millis(100));
 
-    let _ = orbita2d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::<bool>{motor_a:true, motor_b:true})?;
+    // let _ = orbita2d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::<bool>{motor_a:true, motor_b:true})?;
     thread::sleep(Duration::from_millis(1000));
     // let torque = orbita3d_poulpe::read_torque_enable(&io, serial_port.as_mut(), id)?;
     // println!("torque: {:?}", torque);
@@ -72,6 +72,7 @@ fn main() -> Result<(), Box<dyn Error>> {
 
     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
@@ -62,7 +62,8 @@ fn main() -> Result<(), Box<dyn Error>> {
     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::<bool>{top:true, middle:true, bottom:true})?;
+    // let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::<bool>{top:true, middle:true, bottom:true})?;
+    let _ = orbita3d_poulpe::write_torque_enable(&io, serial_port.as_mut(), id, MotorValue::<bool>{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<dyn Error>> {
     // 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
@@ -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::<f32>);
-reg_read_write!(torque_flux_limit, 14, MotorValue::<f32>);
-reg_read_write!(uq_ud_limit, 18, MotorValue::<f32>);
+reg_read_write!(velocity_limit, 10, MotorValue::<u32>);
+reg_read_write!(torque_flux_limit, 14, MotorValue::<u16>);
+reg_read_write!(uq_ud_limit, 18, MotorValue::<i16>);
 
 reg_read_write!(flux_pid, 20, MotorValue::<Pid>);
 reg_read_write!(torque_pid, 24, MotorValue::<Pid>);
@@ -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::<f32>::from_le_bytes(bytes[0..12].try_into().unwrap()),
-			speed: MotorValue::<f32>::from_le_bytes(bytes[12..24].try_into().unwrap()),
-			load: MotorValue::<f32>::from_le_bytes(bytes[24..36].try_into().unwrap()),
+			position: MotorValue::<f32>::from_le_bytes(bytes[0..8].try_into().unwrap()),
+			speed: MotorValue::<f32>::from_le_bytes(bytes[8..16].try_into().unwrap()),
+			load: MotorValue::<f32>::from_le_bytes(bytes[16..24].try_into().unwrap()),
 		}
 	}
 	// pub fn to_le_bytes(&self) -> [u8; 36] {
@@ -97,6 +97,79 @@ impl MotorValue<f32> {
 }
 
 
+
+impl MotorValue<u32> {
+    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<i32> {
+    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<i16> {
+    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<u16> {
+    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<bool> {
     pub fn from_le_bytes(bytes: [u8; 2]) -> Self {
         MotorValue {

diff --git 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::<f32>);
-reg_read_write!(torque_flux_limit, 14, MotorValue::<f32>);
-reg_read_write!(uq_ud_limit, 18, MotorValue::<f32>);
+reg_read_write!(velocity_limit, 10, MotorValue::<u32>);
+reg_read_write!(torque_flux_limit, 14, MotorValue::<u16>);
+reg_read_write!(uq_ud_limit, 18, MotorValue::<i16>);
 
 reg_read_write!(flux_pid, 20, MotorValue::<Pid>);
 reg_read_write!(torque_pid, 24, MotorValue::<Pid>);
@@ -108,6 +108,84 @@ impl MotorValue<f32> {
 }
 
 
+impl MotorValue<u32> {
+    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<i32> {
+    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<i16> {
+    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<u16> {
+    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<bool> {
     pub fn from_le_bytes(bytes: [u8; 3]) -> Self {
         MotorValue {
@@ -196,15 +274,15 @@ impl Vec3d<f32> {
 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()
     }