From 5f19dc687d0d172847787c22f1953d4e962b1f02 Mon Sep 17 00:00:00 2001
From: Pierre Rouanet <pierre.rouanet@gmail.com>
Date: Wed, 15 Nov 2023 11:49:18 +0100
Subject: [PATCH] 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<T> {
+    pub motor_a: T,
+    pub motor_b: T,
+}
+
+reg_read_write!(torque_enable, 40, ValuePerMotor::<u8>);
+reg_read_write!(present_position, 50, ValuePerMotor::<f32>);
+reg_read_write!(goal_position, 60, ValuePerMotor::<f32>);
+
+impl ValuePerMotor<u8> {
+    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<f32> {
+    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<T> {
+    pub top: T,
+    pub middle: T,
+    pub bottom: T,
+}
+
+reg_read_write!(torque_enable, 40, ValuePerMotor::<u8>);
+reg_read_write!(present_position, 50, ValuePerMotor::<f32>);
+reg_read_write!(goal_position, 60, ValuePerMotor::<f32>);
+
+impl ValuePerMotor<u8> {
+    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<f32> {
+    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()
+    }
+}