Add offset and reduction.

pollen-robotics · Aug 21, 2023 · fc45bff · fc45bff
1 parent 4c144d9
commit fc45bff
Show file tree

Hide file tree

Showing 3 changed files with 238 additions and 46 deletions.
diff --git a/src/fake_motor.rs b/src/fake_motor.rs
@@ -4,6 +4,9 @@ use crate::{MotorController, Result, PID};
 
 /// Fake motor implementation for testing purposes.
 pub struct FakeMotor {
+    offset: f64,
+    reduction_ratio: f64,
+
     torque_on: bool,
 
     current_position: f64,
@@ -20,6 +23,9 @@ pub struct FakeMotor {
 impl Default for FakeMotor {
     fn default() -> Self {
         Self {
+            offset: 0.0,
+            reduction_ratio: 1.0,
+
             torque_on: false,
 
             current_position: 0.0,
@@ -44,6 +50,14 @@ impl MotorController for FakeMotor {
         "FakeMotor".to_string()
     }
 
+    fn get_offset(&mut self) -> f64 {
+        self.offset
+    }
+
+    fn get_reduction_ratio(&mut self) -> f64 {
+        self.reduction_ratio
+    }
+
     fn is_torque_on(&mut self) -> Result<bool> {
         Ok(self.torque_on)
     }
@@ -56,23 +70,22 @@ impl MotorController for FakeMotor {
         Ok(())
     }
 
-    fn get_current_position(&mut self) -> Result<f64> {
+    fn get_raw_position(&mut self) -> Result<f64> {
         Ok(self.current_position)
     }
 
-    fn get_current_velocity(&mut self) -> Result<f64> {
+    fn get_raw_velocity(&mut self) -> Result<f64> {
         Ok(self.current_velocity)
     }
 
-    fn get_current_torque(&mut self) -> Result<f64> {
+    fn get_raw_torque(&mut self) -> Result<f64> {
         Ok(self.current_torque)
     }
 
-    fn get_target_position(&mut self) -> Result<f64> {
+    fn get_raw_target_position(&mut self) -> Result<f64> {
         Ok(self.target_position)
     }
-
-    fn set_target_position(&mut self, position: f64) -> Result<()> {
+    fn set_raw_target_position(&mut self, position: f64) -> Result<()> {
         self.target_position = position;
 
         if self.torque_on {
@@ -82,20 +95,20 @@ impl MotorController for FakeMotor {
         Ok(())
     }
 
-    fn get_velocity_limit(&mut self) -> Result<f64> {
+    fn get_raw_velocity_limit(&mut self) -> Result<f64> {
         Ok(self.velocity_limit)
     }
 
-    fn set_velocity_limit(&mut self, velocity: f64) -> Result<()> {
+    fn set_raw_velocity_limit(&mut self, velocity: f64) -> Result<()> {
         self.velocity_limit = velocity;
         Ok(())
     }
 
-    fn get_torque_limit(&mut self) -> Result<f64> {
+    fn get_raw_torque_limit(&mut self) -> Result<f64> {
         Ok(self.torque_limit)
     }
 
-    fn set_torque_limit(&mut self, torque: f64) -> Result<()> {
+    fn set_raw_torque_limit(&mut self, torque: f64) -> Result<()> {
         self.torque_limit = torque;
         Ok(())
     }

diff --git a/src/motor_controller.rs b/src/motor_controller.rs
@@ -8,6 +8,11 @@ pub trait MotorController {
     /// Name of the controller (used for Debug trait)
     fn name(&self) -> String;
 
+    /// Get motor offset (in radians)
+    fn get_offset(&mut self) -> f64;
+    /// Get motor reduction ratio
+    fn get_reduction_ratio(&mut self) -> f64;
+
     /// Check if the motor is ON or OFF
     fn is_torque_on(&mut self) -> Result<bool>;
     /// Enable/Disable the torque
@@ -32,26 +37,78 @@ pub trait MotorController {
     }
 
     /// Get the current position of the motor (in radians)
-    fn get_current_position(&mut self) -> Result<f64>;
+    fn get_current_position(&mut self) -> Result<f64> {
+        Ok(self.get_raw_position()? / self.get_reduction_ratio() - self.get_offset())
+    }
+    /// Get the motor raw position (in radians)
+    /// ie. without offset and reduction ratio
+    fn get_raw_position(&mut self) -> Result<f64>;
+
     /// Get the current velocity of the motor (in radians per second)
-    fn get_current_velocity(&mut self) -> Result<f64>;
+    fn get_current_velocity(&mut self) -> Result<f64> {
+        Ok(self.get_raw_velocity()? / self.get_reduction_ratio())
+    }
+    /// Get the motor raw velocity (in radians per second)
+    /// ie. without reduction ratio
+    fn get_raw_velocity(&mut self) -> Result<f64>;
+
     /// Get the current torque of the motor (in Nm)
-    fn get_current_torque(&mut self) -> Result<f64>;
+    fn get_current_torque(&mut self) -> Result<f64> {
+        Ok(self.get_raw_torque()? / self.get_reduction_ratio())
+    }
+    /// Get the motor raw torque (in Nm)
+    /// ie. without reduction ratio
+    fn get_raw_torque(&mut self) -> Result<f64>;
 
     /// Get the current target position of the motor (in radians)
-    fn get_target_position(&mut self) -> Result<f64>;
+    fn get_target_position(&mut self) -> Result<f64> {
+        Ok(self.get_raw_target_position()? / self.get_reduction_ratio() - self.get_offset())
+    }
+    /// Get raw target position of the motor (in radians)
+    /// ie. without offset and reduction ratio
+    fn get_raw_target_position(&mut self) -> Result<f64>;
+
     /// Set the current target position of the motor (in radians)
-    fn set_target_position(&mut self, position: f64) -> Result<()>;
+    fn set_target_position(&mut self, position: f64) -> Result<()> {
+        let raw_target_position = position * self.get_reduction_ratio() + self.get_offset();
+        self.set_raw_target_position(raw_target_position)
+    }
+    /// Set raw target position of the motor (in radians)
+    /// ie. without offset and reduction ratio
+    fn set_raw_target_position(&mut self, position: f64) -> Result<()>;
 
     /// Get the velocity limit of the motor (in radians per second)
-    fn get_velocity_limit(&mut self) -> Result<f64>;
+    fn get_velocity_limit(&mut self) -> Result<f64> {
+        Ok(self.get_raw_velocity_limit()? / self.get_reduction_ratio())
+    }
+    /// Get raw velocity limit of the motor (in radians per second)
+    /// ie. without reduction ratio
+    fn get_raw_velocity_limit(&mut self) -> Result<f64>;
     /// Set the velocity limit of the motor (in radians per second)
-    fn set_velocity_limit(&mut self, velocity: f64) -> Result<()>;
+    fn set_velocity_limit(&mut self, velocity: f64) -> Result<()> {
+        let raw_velocity_limit = velocity * self.get_reduction_ratio();
+        self.set_raw_velocity_limit(raw_velocity_limit)
+    }
+    /// Set raw velocity limit of the motor (in radians per second)
+    /// ie. without reduction ratio
+    fn set_raw_velocity_limit(&mut self, velocity: f64) -> Result<()>;
 
     /// Get the torque limit of the motor (in Nm)
-    fn get_torque_limit(&mut self) -> Result<f64>;
+    fn get_torque_limit(&mut self) -> Result<f64> {
+        Ok(self.get_raw_torque_limit()? / self.get_reduction_ratio())
+    }
+    /// Get raw torque limit of the motor (in Nm)
+    /// ie. without reduction ratio
+    fn get_raw_torque_limit(&mut self) -> Result<f64>;
+
     /// Set the torque limit of the motor (in Nm)
-    fn set_torque_limit(&mut self, torque: f64) -> Result<()>;
+    fn set_torque_limit(&mut self, torque: f64) -> Result<()> {
+        let raw_torque_limit = torque * self.get_reduction_ratio();
+        self.set_raw_torque_limit(raw_torque_limit)
+    }
+    /// Set raw torque limit of the motor (in Nm)
+    /// ie. without reduction ratio
+    fn set_raw_torque_limit(&mut self, torque: f64) -> Result<()>;
 
     /// Get the current PID gains of the motor
     fn get_pid_gains(&mut self) -> Result<PID>;