calculators share a common trait now

lib/motors/Cargo.toml

@@ -7,4 +7,5 @@ edition = "2021"
 embassy-sync = { version = "0.6.0", features = ["defmt"], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"}
 embassy-time = { version = "0.3.1", features = ["defmt", "defmt-timestamp-uptime", "tick-hz-32_768"], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"}
 heapless = { version = "0.8", default-features = false, features = ["serde"] }
-hyped_core = { path = "../core" }
+hyped_core = { path = "../core" }
+libm = "0.2.11"

lib/motors/src/constant_frequency_calculator.rs

@@ -1,3 +1,5 @@
+use crate::frequency_calculator::{FrequencyCalculator, FrequencyError};
+
 /// Calculator which takes in a frequency, and always returns the frequency it was initialised with.
 pub struct ConstantFrequencyCalculator {
     frequency: u32,
@@ -7,8 +9,10 @@ impl ConstantFrequencyCalculator {
     pub fn new(frequency: u32) -> Self {
         ConstantFrequencyCalculator { frequency }
     }
+}
 
-    pub fn calculate_frequency(&self) -> u32 {
-        self.frequency
+impl FrequencyCalculator for ConstantFrequencyCalculator {
+    fn calculate_frequency(&self, _velocity: f32) -> Result<u32, FrequencyError> {
+        Ok(self.frequency)
     }
 }

lib/motors/src/frequency_calculator.rs

@@ -0,0 +1,8 @@
+pub enum FrequencyError {
+    Negative(f32),
+    Overflow(f32),
+}
+
+pub trait FrequencyCalculator {
+    fn calculate_frequency(&self, velocity: f32) -> Result<u32, FrequencyError>;
+}

lib/motors/src/lib.rs

@@ -1,5 +1,6 @@
 #![no_std]
 
 pub mod constant_frequency_calculator;
+pub mod frequency_calculator;
 pub mod time_frequency_calculator;
 pub mod velocity_frequency_calculator;

lib/motors/src/time_frequency_calculator.rs

@@ -1,3 +1,4 @@
+use crate::frequency_calculator::{FrequencyCalculator, FrequencyError};
 use embassy_time::Instant;
 use heapless::Vec;
 
@@ -9,30 +10,33 @@ use heapless::Vec;
 ///
 /// Returns the frequency corresponding to the time elapsed since the calculator was created.
 pub struct TimeFrequencyCalculator {
-    frequency_table: Vec<Vec<u32, 2>, 256>,
+    frequency_table: Vec<(u32, u32), 256>,
     start_time: u32,
 }
 
 impl TimeFrequencyCalculator {
-    pub fn new(frequency_table: Vec<Vec<u32, 2>, 256>) -> Self {
+    pub fn new(frequency_table: Vec<(u32, u32), 256>) -> Self {
         let start_time = Instant::now().as_micros();
         TimeFrequencyCalculator {
             start_time: start_time as u32,
             frequency_table,
         }
     }
+    pub fn reset(&mut self) {
+        self.start_time = Instant::now().as_micros() as u32;
+    }
+}
 
-    pub fn calculate_frequency(&self) -> u32 {
+impl FrequencyCalculator for TimeFrequencyCalculator {
+    fn calculate_frequency(&self, _velocity: f32) -> Result<u32, FrequencyError> {
         let microseconds_elapsed = Instant::now().as_micros() as u32 - self.start_time;
-        self.frequency_table
+        let freq = self
+            .frequency_table
             .iter()
             .rev()
-            .find(|pair| pair[0] < microseconds_elapsed)
-            .map(|pair| pair[1])
-            .unwrap_or(0)
-    }
-
-    pub fn reset(&mut self) {
-        self.start_time = Instant::now().as_micros() as u32;
+            .find(|(time, _)| *time < microseconds_elapsed)
+            .map(|(_, frequency)| *frequency)
+            .unwrap_or(0);
+        Ok(freq)
     }
 }

lib/motors/src/velocity_frequency_calculator.rs

@@ -1,3 +1,6 @@
+use crate::frequency_calculator::{FrequencyCalculator, FrequencyError};
+use libm::powf;
+
 /// Calculates the frequency by taking in a velocity and using a polynomial to calculate the frequency.
 /// The polynomial is defined by the coefficients array
 ///
@@ -6,20 +9,17 @@ pub struct VelocityFrequencyCalculator {
     coefficients: [f32; 5],
 }
 
-pub enum FrequencyError {
-    Negative(f32),
-    Overflow(f32),
-}
-
 impl VelocityFrequencyCalculator {
     pub fn new(coefficients: [f32; 5]) -> Self {
         VelocityFrequencyCalculator { coefficients }
     }
+}
 
-    pub fn calculate_frequency(&self, velocity: f32) -> Result<u32, FrequencyError> {
-        let frequency = velocity * velocity * velocity * velocity * self.coefficients[0]
-            + velocity * velocity * velocity * self.coefficients[1]
-            + velocity * velocity * self.coefficients[2]
+impl FrequencyCalculator for VelocityFrequencyCalculator {
+    fn calculate_frequency(&self, velocity: f32) -> Result<u32, FrequencyError> {
+        let frequency = powf(velocity, 4.0) * self.coefficients[0]
+            + powf(velocity, 3.0) * self.coefficients[1]
+            + powf(velocity, 2.0) * self.coefficients[2]
             + velocity * self.coefficients[3]
             + self.coefficients[4];