Merge pull request #30 from pollen-robotics/29-add-target_torque

29 add target torque

src/fake_motor.rs

@@ -1,5 +1,3 @@
-use std::f64::{INFINITY, NAN};
-
 use itertools::izip;
 
 use crate::motors_controller::MotorsController;
@@ -76,6 +74,9 @@ pub struct FakeMotorsIO<const N: usize> {
     current_torque: [f64; N],
 
     target_position: [f64; N],
+    target_torque: [f64; N],
+    target_velocity: [f64; N],
+    control_mode: [u8; N],
 
     velocity_limit: [f64; N],
     torque_limit: [f64; N],
@@ -88,17 +89,21 @@ impl<const N: usize> Default for FakeMotorsIO<N> {
             torque_on: [false; N],
 
             current_position: [0.0; N],
-            current_velocity: [NAN; N],
-            current_torque: [NAN; N],
+            current_velocity: [f64::NAN; N],
+            current_torque: [f64::NAN; N],
 
             target_position: [0.0; N],
+            target_torque: [f64::NAN; N],
+            target_velocity: [f64::NAN; N],
+
+            control_mode: [0; N],
 
-            velocity_limit: [INFINITY; N],
-            torque_limit: [INFINITY; N],
+            velocity_limit: [f64::INFINITY; N],
+            torque_limit: [f64::INFINITY; N],
             pid: [PID {
-                p: NAN,
-                i: NAN,
-                d: NAN,
+                p: f64::NAN,
+                i: f64::NAN,
+                d: f64::NAN,
             }; N],
         }
     }
@@ -145,6 +150,60 @@ impl<const N: usize> RawMotorsIO<N> for FakeMotorsIO<N> {
         Ok(self.target_position)
     }
 
+    fn get_target_torque(&mut self) -> Result<[f64; N]> {
+        Ok(self.target_torque)
+    }
+
+    fn set_target_torque(&mut self, target_torque: [f64; N]) -> Result<()> {
+        log::debug!(target: "fake_io::set_target_torque", "Setting target_torque to {:?}", target_torque);
+        self.target_torque = target_torque;
+
+        for (cur, on, target) in izip!(&mut self.current_torque, self.torque_on, target_torque) {
+            if on {
+                log::debug!(target: "fake_io::set_target_torque", "Setting current torque to target torque {:?} (torque on)", target);
+                *cur = target;
+            } else {
+                log::debug!(target: "fake_io::set_target_torque", "Current torque unchanged (torque off)");
+            }
+        }
+
+        Ok(())
+    }
+
+    fn get_target_velocity(&mut self) -> Result<[f64; N]> {
+        Ok(self.target_velocity)
+    }
+
+    fn set_target_velocity(&mut self, target_velocity: [f64; N]) -> Result<()> {
+        log::debug!(target: "fake_io::set_target_velocity", "Setting target_velocity to {:?}", target_velocity);
+        self.target_velocity = target_velocity;
+
+        for (cur, on, target) in izip!(&mut self.current_velocity, self.torque_on, target_velocity)
+        {
+            if on {
+                log::debug!(target: "fake_io::set_target_velocity", "Setting current velocity to target velocity {:?} (velocity on)", target);
+                *cur = target;
+            } else {
+                log::debug!(target: "fake_io::set_target_velocity", "Current velocity unchanged (velocity off)");
+            }
+        }
+
+        Ok(())
+    }
+
+    fn get_control_mode(&mut self) -> Result<[u8; N]> {
+        Ok(self.control_mode)
+    }
+
+    fn set_control_mode(&mut self, control_mode: [u8; N]) -> Result<()> {
+        log::debug!(target: "fake_io::set_control_mode", "Setting control_mode to {:?}", control_mode);
+        self.control_mode = control_mode;
+
+        log::debug!(target: "fake_io::set_control_mode", "Setting control_mode to {:?}",control_mode);
+
+        Ok(())
+    }
+
     fn set_target_position(&mut self, target_position: [f64; N]) -> Result<()> {
         log::debug!(target: "fake_io::set_target_position", "Setting target_position to {:?}", target_position);
         self.target_position = target_position;

src/lib.rs

@@ -1,4 +1,4 @@
-#![feature(generic_const_exprs)]
+// #![feature(generic_const_exprs)]
 #![allow(incomplete_features)]
 
 mod fake_motor;
@@ -10,7 +10,7 @@ pub use limit::Limit;
 mod motors_io;
 pub use motors_io::RawMotorsIO;
 mod motors_controller;
-pub use motors_controller::{MissingResisterErrror, MotorsController};
+pub use motors_controller::{MissingRegisterErrror, MotorsController};
 
 mod pid;
 pub use pid::PID;

src/motors_controller.rs

@@ -119,6 +119,48 @@ pub trait MotorsController<const N: usize> {
         self.io().set_target_position(limited_position)
     }
 
+    /// Set the current target torque of the motors (in Nm)
+    fn set_target_torque(&mut self, torque: [f64; N]) -> Result<()> {
+        log::debug!(target: "controller::set_target_torque", "real target_torque: {:?}", torque);
+
+        self.io().set_target_torque(torque)
+    }
+
+    /// Set the current target velocity of the motors (in rad/s)
+    fn set_target_velocity(&mut self, velocity: [f64; N]) -> Result<()> {
+        log::debug!(target: "controller::set_target_velocity", "real target_velocity: {:?}", velocity);
+
+        self.io().set_target_velocity(velocity)
+    }
+
+    /// Set control mode
+    fn set_control_mode(&mut self, mode: [u8; N]) -> Result<()> {
+        log::debug!(target: "controller::set_control_mode", "real control_mode: {:?}", mode);
+
+        self.io().set_control_mode(mode)
+    }
+
+    /// Get the current target torque of the motors (in Nm)
+    fn get_target_torque(&mut self) -> Result<[f64; N]> {
+        let torque = self.io().get_target_torque()?;
+        log::debug!(target: "controller::get_target_torque", "raw target_torque: {:?}", torque);
+        Ok(torque)
+    }
+
+    /// Get the current target velocity of the motors (in rad/s)
+    fn get_target_velocity(&mut self) -> Result<[f64; N]> {
+        let velocity = self.io().get_target_velocity()?;
+        log::debug!(target: "controller::get_target_velocity", "raw target_velocity: {:?}", velocity);
+        Ok(velocity)
+    }
+
+    /// Get the current control mode
+    fn get_control_mode(&mut self) -> Result<[u8; N]> {
+        let mode = self.io().get_control_mode()?;
+        log::debug!(target: "controller::get_control_mode", "raw control_mode: {:?}", mode);
+        Ok(mode)
+    }
+
     /// Set the current target position and returns the motor feeback (position, velocity, torque)
     fn set_target_position_fb(&mut self, position: [f64; N]) -> Result<[f64; N]> {
         log::debug!(target: "controller::set_target_position", "real target_position: {:?}", position);
@@ -251,11 +293,11 @@ pub trait MotorsController<const N: usize> {
 }
 
 #[derive(Debug)]
-pub struct MissingResisterErrror(pub String);
-impl std::fmt::Display for MissingResisterErrror {
+pub struct MissingRegisterErrror(pub String);
+impl std::fmt::Display for MissingRegisterErrror {
     fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
         let name = &self.0;
         write!(f, "(missing register \"{name}\")",)
     }
 }
-impl std::error::Error for MissingResisterErrror {}
+impl std::error::Error for MissingRegisterErrror {}

src/motors_io.rs

@@ -18,6 +18,23 @@ pub trait RawMotorsIO<const N: usize> {
     /// Set the current target position of the motors (in radians)
     fn set_target_position(&mut self, position: [f64; N]) -> Result<()>;
 
+    /// Get the current target torque of the motors (in Nm)
+    fn get_target_torque(&mut self) -> Result<[f64; N]>;
+    /// Set the current target torque of the motors (in Nm)
+    fn set_target_torque(&mut self, torque: [f64; N]) -> Result<()>;
+
+    /// Set the current target velocity of the motors (in rad/s)
+    fn set_target_velocity(&mut self, velocity: [f64; N]) -> Result<()>;
+
+    /// Get the current target velocity of the motors (in rad/s)
+    fn get_target_velocity(&mut self) -> Result<[f64; N]>;
+
+    /// Set the control mode
+    fn set_control_mode(&mut self, mode: [u8; N]) -> Result<()>;
+
+    /// Get the control mode
+    fn get_control_mode(&mut self) -> Result<[u8; N]>;
+
     /// Set the current target position and returns the motor feeback (position, velocity, torque)
     fn set_target_position_fb(&mut self, position: [f64; N]) -> Result<[f64; N]>;