CTL - Motor Controller Calculators

Cargo.toml

@@ -5,6 +5,7 @@ members = [
   "lib/control",
   "lib/io/*",
   "lib/localisation",
+  "lib/motors",
   "lib/sensors"
 ]
 exclude = [

lib/motors/Cargo.toml

@@ -0,0 +1,10 @@
+[package]
+name = "hyped_motors"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+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" }

lib/motors/src/constant_frequency_calculator.rs

@@ -0,0 +1,14 @@
+/// 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 }
+    }
+
+    pub fn calculate_frequency(&self) -> u32 {
+        self.frequency
+    }
+}

lib/motors/src/lib.rs

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

lib/motors/src/time_frequency_calculator.rs

@@ -0,0 +1,38 @@
+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<Vec<u32, 2>, 256>,
+    start_time: u32,
+}
+
+impl TimeFrequencyCalculator {
+    pub fn new(frequency_table: Vec<Vec<u32, 2>, 256>) -> Self {
+        let start_time = Instant::now().as_micros();
+        TimeFrequencyCalculator {
+            start_time: start_time as u32,
+            frequency_table,
+        }
+    }
+
+    pub fn calculate_frequency(&self) -> u32 {
+        let microseconds_elapsed = Instant::now().as_micros() as u32 - self.start_time;
+        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;
+    }
+}

lib/motors/src/velocity_frequency_calculator.rs

@@ -0,0 +1,36 @@
+/// 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],
+}
+
+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]
+            + 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)
+    }
+}