diff --git a/Cargo.lock b/Cargo.lock index c902218b..fb12c9c1 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1,6 +1,6 @@ # This file is automatically @generated by Cargo. # It is not intended for manual editing. -version = 4 +version = 3 [[package]] name = "anyhow" @@ -81,9 +81,9 @@ dependencies = [ [[package]] name = "document-features" -version = "0.2.10" +version = "0.2.11" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cb6969eaabd2421f8a2775cfd2471a2b634372b4a25d41e3bd647b79912850a0" +checksum = "95249b50c6c185bee49034bcb378a49dc2b5dff0be90ff6616d31d64febab05d" dependencies = [ "litrs", ] @@ -129,7 +129,7 @@ dependencies = [ [[package]] name = "embassy-sync" version = "0.6.2" -source = "git+https://github.com/embassy-rs/embassy#c65b6db318da7ecbe888a0a66b85d9ffb28106f0" +source = "git+https://github.com/embassy-rs/embassy#f6b480ef1c2ba421353a674cdaa5e52372d1273f" dependencies = [ "cfg-if", "critical-section", @@ -234,9 +234,9 @@ dependencies = [ [[package]] name = "equivalent" -version = "1.0.1" +version = "1.0.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5443807d6dff69373d433ab9ef5378ad8df50ca6298caf15de6e52e24aaf54d5" +checksum = "877a4ace8713b0bcf2a4e7eec82529c029f1d0619886d18145fea96c3ffe5c0f" [[package]] name = "futures-core" @@ -380,6 +380,7 @@ dependencies = [ "heapless", "hyped_can", "hyped_core", + "libm", ] [[package]] @@ -612,9 +613,9 @@ dependencies = [ [[package]] name = "ryu" -version = "1.0.18" +version = "1.0.19" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f3cb5ba0dc43242ce17de99c180e96db90b235b8a9fdc9543c96d2209116bd9f" +checksum = "6ea1a2d0a644769cc99faa24c3ad26b379b786fe7c36fd3c546254801650e6dd" [[package]] name = "serde" @@ -685,9 +686,9 @@ checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3" [[package]] name = "syn" -version = "2.0.96" +version = "2.0.98" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d5d0adab1ae378d7f53bdebc67a39f1f151407ef230f0ce2883572f5d8985c80" +checksum = "36147f1a48ae0ec2b5b3bc5b537d267457555a10dc06f3dbc8cb11ba3006d3b1" dependencies = [ "proc-macro2", "quote", @@ -722,9 +723,9 @@ checksum = "42ff0bf0c66b8238c6f3b578df37d0b7848e55df8577b3f74f92a69acceeb825" [[package]] name = "unicode-ident" -version = "1.0.14" +version = "1.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "adb9e6ca4f869e1180728b7950e35922a7fc6397f7b641499e8f3ef06e50dc83" +checksum = "a210d160f08b701c8721ba1c726c11662f877ea6b7094007e1ca9a1041945034" [[package]] name = "version_check" diff --git a/lib/motors/Cargo.toml b/lib/motors/Cargo.toml index 71698e1d..aa92f558 100644 --- a/lib/motors/Cargo.toml +++ b/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" } \ No newline at end of file +libm = "0.2.11" +hyped_can = { path = "../io/hyped_can" } diff --git a/lib/motors/src/constant_frequency_calculator.rs b/lib/motors/src/constant_frequency_calculator.rs new file mode 100644 index 00000000..fbab1fe6 --- /dev/null +++ b/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 { + Ok(self.frequency) + } +} diff --git a/lib/motors/src/frequency_calculator.rs b/lib/motors/src/frequency_calculator.rs new file mode 100644 index 00000000..91099372 --- /dev/null +++ b/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; +} diff --git a/lib/motors/src/lib.rs b/lib/motors/src/lib.rs index 37a44b99..987cc20d 100644 --- a/lib/motors/src/lib.rs +++ b/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; diff --git a/lib/motors/src/time_frequency_calculator.rs b/lib/motors/src/time_frequency_calculator.rs new file mode 100644 index 00000000..357a95d1 --- /dev/null +++ b/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 { + 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) + } +} diff --git a/lib/motors/src/velocity_frequency_calculator.rs b/lib/motors/src/velocity_frequency_calculator.rs new file mode 100644 index 00000000..bdb54144 --- /dev/null +++ b/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 { + 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) + } +}