Merge pull request #21 from pollen-robotics/offset-reduction

Offset reduction

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "motor_toolbox_rs"
-version = "0.1.0"
+version = "0.2.0"
 edition = "2021"
 license = "Apache-2.0"
 authors = ["Pollen Robotics"]
@@ -14,4 +14,5 @@ categories = ["science::robotics"]
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
+itertools = "0.11.0"
 serde = { version = "1.0.183", features = ["derive"] }

src/fake_motor.rs

@@ -1,162 +1,223 @@
 use std::f64::{INFINITY, NAN};
 
-use crate::{MotorController, Result, PID};
+use itertools::izip;
 
-/// Fake motor implementation for testing purposes.
-pub struct FakeMotor {
-    torque_on: bool,
+use crate::motors_controller::MotorsController;
+use crate::motors_io::RawMotorsIO;
+use crate::{Limit, Result, PID};
 
-    current_position: f64,
-    current_velocity: f64,
-    current_torque: f64,
+pub struct FakeMotorsController<const N: usize> {
+    offsets: [Option<f64>; N],
+    reduction: [Option<f64>; N],
+    limits: [Option<Limit>; N],
 
-    target_position: f64,
+    io: FakeMotorsIO<N>,
+}
+
+impl<const N: usize> FakeMotorsController<N> {
+    pub fn new() -> Self {
+        Self::default()
+    }
+
+    pub fn with_offsets(mut self, offsets: [Option<f64>; N]) -> Self {
+        self.offsets = offsets;
+        self
+    }
+
+    pub fn with_reduction(mut self, reduction: [Option<f64>; N]) -> Self {
+        self.reduction = reduction;
+        self
+    }
+
+    pub fn with_limits(mut self, limits: [Option<Limit>; N]) -> Self {
+        self.limits = limits;
+        self
+    }
+}
+
+impl<const N: usize> Default for FakeMotorsController<N> {
+    fn default() -> Self {
+        Self {
+            offsets: [None; N],
+            reduction: [None; N],
+            limits: [None; N],
+
+            io: FakeMotorsIO::<N>::default(),
+        }
+    }
+}
+
+impl<const N: usize> MotorsController<N> for FakeMotorsController<N> {
+    fn offsets(&self) -> [Option<f64>; N] {
+        self.offsets
+    }
+
+    fn reduction(&self) -> [Option<f64>; N] {
+        self.reduction
+    }
+
+    fn limits(&self) -> [Option<Limit>; N] {
+        self.limits
+    }
+
+    fn io(&mut self) -> &mut dyn RawMotorsIO<N> {
+        &mut self.io
+    }
+}
+
+/// Fake motor io implementation for testing purposes.
+pub struct FakeMotorsIO<const N: usize> {
+    torque_on: [bool; N],
 
-    velocity_limit: f64,
-    torque_limit: f64,
-    pid: PID,
+    current_position: [f64; N],
+    current_velocity: [f64; N],
+    current_torque: [f64; N],
+
+    target_position: [f64; N],
+
+    velocity_limit: [f64; N],
+    torque_limit: [f64; N],
+    pid: [PID; N],
 }
 
-impl Default for FakeMotor {
+impl<const N: usize> Default for FakeMotorsIO<N> {
     fn default() -> Self {
         Self {
-            torque_on: false,
+            torque_on: [false; N],
 
-            current_position: 0.0,
-            current_velocity: NAN,
-            current_torque: NAN,
+            current_position: [0.0; N],
+            current_velocity: [NAN; N],
+            current_torque: [NAN; N],
 
-            target_position: 0.0,
+            target_position: [0.0; N],
 
-            velocity_limit: INFINITY,
-            torque_limit: INFINITY,
-            pid: PID {
+            velocity_limit: [INFINITY; N],
+            torque_limit: [INFINITY; N],
+            pid: [PID {
                 p: NAN,
                 i: NAN,
                 d: NAN,
-            },
+            }; N],
         }
     }
 }
 
-impl MotorController for FakeMotor {
-    fn name(&self) -> String {
-        "FakeMotor".to_string()
-    }
-
-    fn is_torque_on(&mut self) -> Result<bool> {
+impl<const N: usize> RawMotorsIO<N> for FakeMotorsIO<N> {
+    fn is_torque_on(&mut self) -> Result<[bool; N]> {
         Ok(self.torque_on)
     }
 
-    fn set_torque(&mut self, on: bool) -> Result<()> {
-        if on != self.torque_on {
-            self.current_position = self.target_position;
+    fn set_torque(&mut self, on: [bool; N]) -> Result<()> {
+        for (cur, target, on, torque_on) in izip!(
+            &mut self.current_position,
+            self.target_position,
+            on,
+            self.torque_on
+        ) {
+            if on && !torque_on {
+                *cur = target;
+            }
         }
+
         self.torque_on = on;
+
         Ok(())
     }
 
-    fn get_current_position(&mut self) -> Result<f64> {
+    fn get_current_position(&mut self) -> Result<[f64; N]> {
         Ok(self.current_position)
     }
 
-    fn get_current_velocity(&mut self) -> Result<f64> {
+    fn get_current_velocity(&mut self) -> Result<[f64; N]> {
         Ok(self.current_velocity)
     }
 
-    fn get_current_torque(&mut self) -> Result<f64> {
+    fn get_current_torque(&mut self) -> Result<[f64; N]> {
         Ok(self.current_torque)
     }
 
-    fn get_target_position(&mut self) -> Result<f64> {
+    fn get_target_position(&mut self) -> Result<[f64; N]> {
         Ok(self.target_position)
     }
 
-    fn set_target_position(&mut self, position: f64) -> Result<()> {
-        self.target_position = position;
+    fn set_target_position(&mut self, target_position: [f64; N]) -> Result<()> {
+        self.target_position = target_position;
 
-        if self.torque_on {
-            self.current_position = position;
+        for (cur, on, targ) in izip!(&mut self.current_position, self.torque_on, target_position) {
+            if on {
+                *cur = targ;
+            }
         }
 
         Ok(())
     }
 
-    fn get_velocity_limit(&mut self) -> Result<f64> {
+    fn get_velocity_limit(&mut self) -> Result<[f64; N]> {
         Ok(self.velocity_limit)
     }
 
-    fn set_velocity_limit(&mut self, velocity: f64) -> Result<()> {
+    fn set_velocity_limit(&mut self, velocity: [f64; N]) -> Result<()> {
         self.velocity_limit = velocity;
         Ok(())
     }
 
-    fn get_torque_limit(&mut self) -> Result<f64> {
+    fn get_torque_limit(&mut self) -> Result<[f64; N]> {
         Ok(self.torque_limit)
     }
 
-    fn set_torque_limit(&mut self, torque: f64) -> Result<()> {
+    fn set_torque_limit(&mut self, torque: [f64; N]) -> Result<()> {
         self.torque_limit = torque;
         Ok(())
     }
 
-    fn get_pid_gains(&mut self) -> Result<crate::PID> {
+    fn get_pid_gains(&mut self) -> Result<[PID; N]> {
         Ok(self.pid)
     }
 
-    fn set_pid_gains(&mut self, pid: crate::PID) -> Result<()> {
+    fn set_pid_gains(&mut self, pid: [PID; N]) -> Result<()> {
         self.pid = pid;
         Ok(())
     }
 }
 
 #[cfg(test)]
 mod tests {
-    use crate::{
-        FakeMotor, MotorController, MultipleMotorsController, MultipleMotorsControllerWrapper,
-    };
+    use crate::{fake_motor::FakeMotorsIO, motors_io::RawMotorsIO};
 
     #[test]
     fn check_default() {
-        let mut motor: FakeMotor = FakeMotor::default();
+        let mut motor = FakeMotorsIO::<1>::default();
 
-        assert!(!motor.is_torque_on().unwrap());
-        assert_eq!(motor.get_current_position().unwrap(), 0.0);
-        assert_eq!(motor.get_target_position().unwrap(), 0.0);
+        assert!(!motor.is_torque_on().unwrap()[0]);
+        assert_eq!(motor.get_current_position().unwrap(), [0.0]);
+        assert_eq!(motor.get_target_position().unwrap(), [0.0]);
     }
 
     #[test]
     fn set_target() {
-        let mut motor: FakeMotor = FakeMotor::default();
+        let mut motor = FakeMotorsIO::<1>::default();
 
         // With torque off, the current position should not change
-        motor.set_target_position(0.5).unwrap();
-        assert_eq!(motor.get_target_position().unwrap(), 0.5);
-        assert_eq!(motor.get_current_position().unwrap(), 0.0);
+        motor.set_target_position([0.5]).unwrap();
+        assert_eq!(motor.get_target_position().unwrap(), [0.5]);
+        assert_eq!(motor.get_current_position().unwrap(), [0.0]);
 
-        // Enabling the torque, and reset the target position
-        motor.enable_torque(true).unwrap();
-        assert!(motor.is_torque_on().unwrap());
-        assert_eq!(motor.get_current_position().unwrap(), 0.0);
-        assert_eq!(motor.get_target_position().unwrap(), 0.0);
+        // Enabling the torque
+        motor.set_torque([true]).unwrap();
+        assert!(motor.is_torque_on().unwrap()[0]);
+        assert_eq!(motor.get_current_position().unwrap(), [0.5]);
+        assert_eq!(motor.get_target_position().unwrap(), [0.5]);
 
         // Setting the target position
-        motor.set_target_position(0.5).unwrap();
-        assert_eq!(motor.get_target_position().unwrap(), 0.5);
-        assert_eq!(motor.get_current_position().unwrap(), 0.5);
+        motor.set_target_position([0.25]).unwrap();
+        assert_eq!(motor.get_target_position().unwrap(), [0.25]);
+        assert_eq!(motor.get_current_position().unwrap(), [0.25]);
     }
 
     #[test]
     fn multiple_fake() {
-        let motor1 = Box::<FakeMotor>::default();
-        let motor2 = Box::<FakeMotor>::default();
-        let motor3 = Box::<FakeMotor>::default();
-
-        let controllers: [Box<dyn MotorController>; 3] = [motor1, motor2, motor3];
-
-        let mut wrapper = MultipleMotorsControllerWrapper::new(controllers);
-
-        wrapper.set_torque(true).unwrap();
-        assert!(wrapper.is_torque_on().unwrap());
+        let mut motors = FakeMotorsIO::<3>::default();
+        motors.set_torque([true, false, true]).unwrap();
+        assert_eq!(motors.is_torque_on().unwrap(), [true, false, true]);
     }
 }

src/lib.rs

@@ -1,16 +1,15 @@
-mod coherency;
-
 mod fake_motor;
-pub use fake_motor::FakeMotor;
+pub use fake_motor::FakeMotorsController;
 
 mod limit;
 pub use limit::Limit;
 
-mod motor_controller;
-pub use motor_controller::{MissingResisterErrror, MotorController, Result};
-
-mod multiple_motors_controller;
-pub use multiple_motors_controller::{MultipleMotorsController, MultipleMotorsControllerWrapper};
+mod motors_io;
+pub use motors_io::RawMotorsIO;
+mod motors_controller;
+pub use motors_controller::{MissingResisterErrror, MotorsController};
 
 mod pid;
 pub use pid::PID;
+
+pub type Result<T> = std::result::Result<T, Box<dyn std::error::Error>>;