CTL - Motor Controller Calculators (#83)

lib/motors/Cargo.toml

@@ -8,4 +8,5 @@ embassy-sync = { version = "0.6.0", features = ["defmt"], git = "https://github.
 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_can = { path = "../io/hyped_can" }
+libm = "0.2.11"
+hyped_can = { path = "../io/hyped_can" }

lib/motors/src/constant_frequency_calculator.rs

@@ -0,0 +1,18 @@
+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,
+}
+
+impl ConstantFrequencyCalculator {
+    pub fn new(frequency: u32) -> Self {
+        ConstantFrequencyCalculator { 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

@@ -2,3 +2,7 @@
 
 pub mod can_open_message;
 pub mod can_open_processor;
+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

@@ -0,0 +1,42 @@
+use crate::frequency_calculator::{FrequencyCalculator, FrequencyError};
+use embassy_time::Instant;
+use heapless::Vec;
+
+/// Takes in a frequency table and calculates the frequency based on the time elapsed since the calculator was created
+/// The frequency table is a list of pairs, where the first element is the time elapsed in microseconds,
+/// and the second element is the frequency to return when the time elapsed is less than the first element.
+///
+/// Useful for creating a frequency that predictably changes over time.
+///
+/// Returns the frequency corresponding to the time elapsed since the calculator was created.
+pub struct TimeFrequencyCalculator {
+    frequency_table: Vec<(u32, u32), 256>,
+    start_time: u32,
+}
+
+impl TimeFrequencyCalculator {
+    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;
+    }
+}
+
+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;
+        let freq = self
+            .frequency_table
+            .iter()
+            .rev()
+            .find(|(time, _)| *time < microseconds_elapsed)
+            .map(|(_, frequency)| *frequency)
+            .unwrap_or(0);
+        Ok(freq)
+    }
+}

lib/motors/src/velocity_frequency_calculator.rs

@@ -0,0 +1,36 @@
+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
+///
+/// Returns the frequency calculated from the velocity or an error if the frequency is negative or overflows
+pub struct VelocityFrequencyCalculator {
+    coefficients: [f32; 5],
+}
+
+impl VelocityFrequencyCalculator {
+    pub fn new(coefficients: [f32; 5]) -> Self {
+        VelocityFrequencyCalculator { coefficients }
+    }
+}
+
+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];
+
+        if frequency < 0.0 {
+            return Err(FrequencyError::Negative(frequency));
+        }
+
+        if frequency > u32::MAX as f32 {
+            return Err(FrequencyError::Overflow(frequency));
+        }
+
+        Ok(frequency as u32)
+    }
+}