const HISTORY_SIZE: usize = 20;
const PAIR_WEIGHTS: [f32; 3] = [0.05, 0.35, 0.6];
pub const ASSUME_STOPPED_MS: i64 = 40;
#[derive(Clone, Copy, Default)]
struct DataPointAtTime {
time_ms: i64,
data_point: f32,
}
#[derive(Clone)]
pub struct VelocityTracker1D {
samples: [Option<DataPointAtTime>; HISTORY_SIZE],
index: usize,
}
impl Default for VelocityTracker1D {
fn default() -> Self {
Self::new()
}
}
impl VelocityTracker1D {
pub fn new() -> Self {
Self {
samples: [None; HISTORY_SIZE],
index: 0,
}
}
pub fn add_data_point(&mut self, time_ms: i64, data_point: f32) {
self.index = (self.index + 1) % HISTORY_SIZE;
self.samples[self.index] = Some(DataPointAtTime {
time_ms,
data_point,
});
}
pub fn calculate_velocity(&self) -> f32 {
let mut velocity = 0.0;
let mut newer_index = self.index;
for weight in PAIR_WEIGHTS {
let older_index = (newer_index + HISTORY_SIZE - 1) % HISTORY_SIZE;
let (Some(newer), Some(older)) = (self.samples[newer_index], self.samples[older_index])
else {
break;
};
let elapsed_ms = newer.time_ms - older.time_ms;
if !(0..=ASSUME_STOPPED_MS).contains(&elapsed_ms) {
break;
}
if elapsed_ms > 0 {
let per_ms = (newer.data_point - older.data_point) / elapsed_ms as f32;
velocity += weight * per_ms * 1000.0;
}
newer_index = older_index;
}
velocity
}
pub fn calculate_velocity_with_max(&self, max_velocity: f32) -> f32 {
if !max_velocity.is_finite() || max_velocity <= 0.0 {
return 0.0;
}
let velocity = self.calculate_velocity();
if velocity == 0.0 || velocity.is_nan() {
return 0.0;
}
velocity.clamp(-max_velocity, max_velocity)
}
pub fn release_velocity(
&mut self,
lift_time_ms: i64,
lift_position: f32,
max_velocity: f32,
) -> f32 {
let Some(newest) = self.samples[self.index] else {
return 0.0;
};
if lift_time_ms - newest.time_ms > ASSUME_STOPPED_MS {
return 0.0;
}
if lift_position != newest.data_point {
self.add_data_point(lift_time_ms, lift_position);
}
self.calculate_velocity_with_max(max_velocity)
}
pub fn reset(&mut self) {
self.samples = [None; HISTORY_SIZE];
self.index = 0;
}
}
#[cfg(test)]
#[path = "tests/velocity_tracker_tests.rs"]
mod tests;