diff --git a/src/filters.rs b/src/filters.rs new file mode 100644 index 0000000..8d5b00d --- /dev/null +++ b/src/filters.rs @@ -0,0 +1,235 @@ +//! Small, self-contained scalar filters used by the motor +//! [`VelocityEstimator`](crate::velocity_estimator). +//! +//! Each filter operates on a stream of `f64` samples fed one at a time through +//! `filter`. The windowed filters ([`Sma`], [`Median`], [`MaxAbs`]) start empty +//! and, until their window fills, operate over however many samples they've seen +//! so far rather than waiting for a full window. +//! +//! This module is deliberately free of any `vexide`/hardware dependency so it can +//! be unit-tested on the host. + +use std::collections::VecDeque; + +/// A simple moving average: the mean of the last `window` samples. +pub struct Sma { + window: usize, + samples: VecDeque, +} + +impl Sma { + pub fn new(window: usize) -> Self { + Self { + window: window.max(1), + samples: VecDeque::with_capacity(window.max(1)), + } + } + + /// Pushes `input` and returns the mean of the samples currently in the + /// window. + pub fn filter(&mut self, input: f64) -> f64 { + if self.samples.len() == self.window { + self.samples.pop_front(); + } + self.samples.push_back(input); + self.samples.iter().sum::() / self.samples.len() as f64 + } +} + +/// A median filter: the middle value of the last `window` samples, sorted. +pub struct Median { + window: usize, + samples: VecDeque, +} + +impl Median { + pub fn new(window: usize) -> Self { + Self { + window: window.max(1), + samples: VecDeque::with_capacity(window.max(1)), + } + } + + /// Pushes `input` and returns the middle of the sorted window (the upper of + /// the two middles while the window holds an even number of samples). + pub fn filter(&mut self, input: f64) -> f64 { + if self.samples.len() == self.window { + self.samples.pop_front(); + } + self.samples.push_back(input); + + let mut sorted: Vec = self.samples.iter().copied().collect(); + sorted.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal)); + sorted[sorted.len() / 2] + } +} + +/// Tracks the largest absolute value seen in the last `window` samples. +pub struct MaxAbs { + window: usize, + samples: VecDeque, +} + +impl MaxAbs { + pub fn new(window: usize) -> Self { + Self { + window: window.max(1), + samples: VecDeque::with_capacity(window.max(1)), + } + } + + /// Pushes `input` and returns the largest `abs()` currently in the window. + pub fn filter(&mut self, input: f64) -> f64 { + if self.samples.len() == self.window { + self.samples.pop_front(); + } + self.samples.push_back(input); + self.samples + .iter() + .map(|value| value.abs()) + .fold(0.0, f64::max) + } +} + +/// An exponential moving average: `state = input * gain + state * (1 - gain)`. +/// +/// The `gain` is supplied per sample rather than stored, so a caller can vary it +/// (as the estimator does with its acceleration-adaptive gain). +#[derive(Default)] +pub struct Ema { + state: f64, +} + +impl Ema { + pub fn new() -> Self { + Self { state: 0.0 } + } + + pub fn filter(&mut self, input: f64, gain: f64) -> f64 { + self.state = input * gain + self.state * (1.0 - gain); + self.state + } +} + +/// A discrete derivative: `(input - previous_input) / dt_ms`. +/// +/// Returns the previous result when `dt_ms <= 0` (no time has passed, so the +/// rate is undefined) and `0.0` for the very first sample (no previous input to +/// difference against). +#[derive(Default)] +pub struct Derivative { + previous_input: Option, + previous_result: f64, +} + +impl Derivative { + pub fn new() -> Self { + Self { + previous_input: None, + previous_result: 0.0, + } + } + + pub fn filter(&mut self, input: f64, dt_ms: f64) -> f64 { + if dt_ms <= 0.0 { + return self.previous_result; + } + let result = match self.previous_input { + Some(previous) => (input - previous) / dt_ms, + None => 0.0, + }; + self.previous_input = Some(input); + self.previous_result = result; + result + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const EPS: f64 = 1e-9; + + #[test] + fn sma_averages_partial_then_full_window() { + let mut sma = Sma::new(3); + // Window fills gradually, averaging over what it holds so far. + assert!((sma.filter(3.0) - 3.0).abs() < EPS); + assert!((sma.filter(5.0) - 4.0).abs() < EPS); + assert!((sma.filter(7.0) - 5.0).abs() < EPS); + // Oldest (3.0) drops out: mean of 5,7,9. + assert!((sma.filter(9.0) - 7.0).abs() < EPS); + } + + #[test] + fn median_returns_middle_of_sorted_window() { + let mut median = Median::new(7); + let mut last = 0.0; + // Values 0,2,4,6,8,10,12 in scrambled order; sorted middle is 6. + for value in [10.0, 2.0, 8.0, 4.0, 6.0, 0.0, 12.0] { + last = median.filter(value); + } + assert!((last - 6.0).abs() < EPS); + } + + #[test] + fn median_uses_partial_window_before_it_fills() { + let mut median = Median::new(7); + // Two samples held: sorted [3, 9], middle index 1 -> 9. + median.filter(9.0); + assert!((median.filter(3.0) - 9.0).abs() < EPS); + } + + #[test] + fn median_rejects_a_single_spike() { + let mut median = Median::new(7); + let mut last = 0.0; + for value in [1.0, 1.0, 1.0, 1000.0, 1.0, 1.0, 1.0] { + last = median.filter(value); + } + // The lone 1000.0 spike is discarded by the median. + assert!((last - 1.0).abs() < EPS); + } + + #[test] + fn max_abs_tracks_largest_magnitude_in_window() { + let mut max_abs = MaxAbs::new(3); + assert!((max_abs.filter(1.0) - 1.0).abs() < EPS); // [1] + assert!((max_abs.filter(-5.0) - 5.0).abs() < EPS); // [1,-5] + assert!((max_abs.filter(2.0) - 5.0).abs() < EPS); // [1,-5,2] + assert!((max_abs.filter(3.0) - 5.0).abs() < EPS); // [-5,2,3] + // The -5.0 finally falls out of the 3-wide window here. + assert!((max_abs.filter(4.0) - 4.0).abs() < EPS); // [2,3,4] + } + + #[test] + fn ema_blends_input_and_state_by_gain() { + let mut ema = Ema::new(); + // gain 1.0 -> follows input exactly. + assert!((ema.filter(10.0, 1.0) - 10.0).abs() < EPS); + // gain 0.5 -> halfway between input and prior state (10.0). + assert!((ema.filter(20.0, 0.5) - 15.0).abs() < EPS); + // gain 0.0 -> holds prior state. + assert!((ema.filter(999.0, 0.0) - 15.0).abs() < EPS); + } + + #[test] + fn derivative_differences_over_dt() { + let mut derivative = Derivative::new(); + // First sample has no predecessor. + assert!((derivative.filter(4.0, 2.0) - 0.0).abs() < EPS); + // (10 - 4) / 2 = 3. + assert!((derivative.filter(10.0, 2.0) - 3.0).abs() < EPS); + } + + #[test] + fn derivative_holds_last_result_when_dt_non_positive() { + let mut derivative = Derivative::new(); + derivative.filter(0.0, 1.0); + let last = derivative.filter(6.0, 1.0); // 6.0 + assert!((last - 6.0).abs() < EPS); + // dt <= 0 returns the previous result and ignores the new input. + assert!((derivative.filter(1000.0, 0.0) - 6.0).abs() < EPS); + assert!((derivative.filter(1000.0, -5.0) - 6.0).abs() < EPS); + } +} diff --git a/src/main.rs b/src/main.rs index 1d71d02..79d842a 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,4 +1,4 @@ -use std::time::Duration; +use std::{cell::RefCell, rc::Rc, time::Duration}; use evian::prelude::*; use vexide::prelude::*; @@ -10,6 +10,11 @@ use evian::{ tracking::wheeled::{TrackingWheel, WheeledTracking}, }; +mod filters; +mod motor_velocity; +mod sensor; +mod velocity_estimator; + mod velocity_differential; use velocity_differential::{MotorGroupVelocity, VelocityDifferential, VelocityDifferentialConfig}; @@ -23,6 +28,18 @@ use sysid::SysIdConfig; /// `Ka` (see `sysid.rs`). Flip back to `false` afterwards. const RUN_SYSID: bool = false; +/// Set to `true` to run the one-shot hardware diagnostic +/// (`sensor::probe_direction`) against the first left-side motor instead of the +/// normal competition code. It reports whether `raw_position()` honors the +/// direction flag and the measured ticks per internal revolution, then exits. +/// Flip back to `false` afterwards. +const RUN_PROBE: bool = false; + +/// Gearset shared by every drive motor. Passed to the velocity trackers instead +/// of read per motor, so a motor that hasn't enumerated yet at power-on can't be +/// silently mis-scaled. +const DRIVE_GEARSET: Gearset = Gearset::Blue; + struct Robot { drivetrain: Drivetrain, WheeledTracking>, @@ -30,20 +47,28 @@ struct Robot { } impl Robot { - const LINEAR_PID: Pid = Pid::new(1.0, 0.0, 0.125, None); - const ANGULAR_PID: AngularPid = AngularPid::new(16.0, 0.0, 1.0, None); + // TODO: tune the outer linear position PID (kp, ki, kd) for this robot. + const LINEAR_PID: Pid = Pid::new(0.0, 0.0, 0.0, None); + // TODO: tune the outer angular (heading) PID (kp, ki, kd) for this robot. + const ANGULAR_PID: AngularPid = AngularPid::new(0.0, 0.0, 0.0, None); + // TODO: set the linear settling tolerances — error (inches), velocity + // (in/s), and settle duration. const LINEAR_TOLERANCES: Tolerances = Tolerances::new() - .error(4.0) - .velocity(0.25) - .duration(Duration::from_millis(15)); + .error(0.0) + .velocity(0.0) + .duration(Duration::from_millis(0)); + // TODO: set the angular settling tolerances — error (radians), velocity + // (rad/s), and settle duration. const ANGULAR_TOLERANCES: Tolerances = Tolerances::new() - .error(f64::to_radians(8.0)) - .velocity(0.09) - .duration(Duration::from_millis(15)); + .error(f64::to_radians(0.0)) + .velocity(0.0) + .duration(Duration::from_millis(0)); /// Full-stick linear velocity for teleop, in inches / second. + // TODO: set the teleop full-stick linear velocity (in/s). const MAX_LINEAR_VELOCITY: f64 = 0.0; /// Full-stick angular velocity for teleop, in radians / second. + // TODO: set the teleop full-stick angular velocity (rad/s). const MAX_ANGULAR_VELOCITY: f64 = 0.0; } @@ -64,21 +89,24 @@ impl Compete for Robot { timeout: Some(Duration::from_secs(10)), }; + // TODO: this is a placeholder demonstration path — replace it with the + // real autonomous routine. Every distance (inches), heading, point, and + // per-call override below is zeroed and needs to be set. basic - .drive_distance(dt, 24.0) - .with_linear_output_limit(6.0) + .drive_distance(dt, 0.0) + .with_linear_output_limit(0.0) .await; basic.turn_to_heading(dt, 0.0.deg()).await; - seeking.move_to_point(dt, (24.0, 24.0)).await; + seeking.move_to_point(dt, (0.0, 0.0)).await; basic - .drive_distance_at_heading(dt, 8.0, 45.0.deg()) - .with_linear_kd(1.2) - .with_angular_tolerance_duration(Duration::from_millis(5)) - .with_angular_error_tolerance(f64::to_radians(10.0)) - .with_linear_error_tolerance(12.0) + .drive_distance_at_heading(dt, 0.0, 0.0.deg()) + .with_linear_kd(0.0) + .with_angular_tolerance_duration(Duration::from_millis(0)) + .with_angular_error_tolerance(f64::to_radians(0.0)) + .with_linear_error_tolerance(0.0) .await; } @@ -107,22 +135,37 @@ async fn main(peripherals: Peripherals) { let forwards_enc = AdiOpticalEncoder::new(peripherals.adi_a, peripherals.adi_b); let sideways_enc = AdiOpticalEncoder::new(peripherals.adi_c, peripherals.adi_d); let mut left_motors = [ - Motor::new(peripherals.port_7, Gearset::Blue, Direction::Forward), - Motor::new(peripherals.port_8, Gearset::Blue, Direction::Reverse), - Motor::new(peripherals.port_9, Gearset::Blue, Direction::Reverse), + Motor::new(peripherals.port_7, DRIVE_GEARSET, Direction::Forward), + Motor::new(peripherals.port_8, DRIVE_GEARSET, Direction::Reverse), + Motor::new(peripherals.port_9, DRIVE_GEARSET, Direction::Reverse), ]; - let mut right_motors = [ - Motor::new(peripherals.port_17, Gearset::Blue, Direction::Reverse), - Motor::new(peripherals.port_18, Gearset::Blue, Direction::Reverse), - Motor::new(peripherals.port_19, Gearset::Blue, Direction::Forward), + let right_motors = [ + Motor::new(peripherals.port_17, DRIVE_GEARSET, Direction::Reverse), + Motor::new(peripherals.port_18, DRIVE_GEARSET, Direction::Reverse), + Motor::new(peripherals.port_19, DRIVE_GEARSET, Direction::Forward), ]; + // Hardware diagnostic: probe one motor for the direction/ticks constants, + // then exit. Runs before the motors are shared, so it never contends for a + // borrow. No drivetrain model or IMU needed. + if RUN_PROBE { + let _ = sensor::probe_direction(&mut left_motors[0]).await; + return; + } + + // Shared ownership of each side's motors: the sysid collector and the + // drivetrain's background velocity trackers both drive these through the + // same `Rc>`. + let left: Rc>> = Rc::new(RefCell::new(left_motors)); + let right: Rc>> = Rc::new(RefCell::new(right_motors)); + // System-identification collector: raw-voltage staircase, no drivetrain // model or IMU needed. Runs to completion, prints Desmos lists, then exits. if RUN_SYSID { sysid::collect( - &mut left_motors, - &mut right_motors, + left.clone(), + right.clone(), + DRIVE_GEARSET, &SysIdConfig { // TODO: set this to the drivetrain's real wheel-per-motor gear // ratio (the same value passed to `VelocityDifferential::new` @@ -141,11 +184,12 @@ async fn main(peripherals: Peripherals) { Robot { drivetrain: Drivetrain::new( VelocityDifferential::new( - left_motors, - right_motors, + left.clone(), + right.clone(), // gear_ratio: wheel revs per motor output-shaft rev; 1.0 for // direct drive. 0.0, + DRIVE_GEARSET, // TODO: characterize the drivetrain and fill these in. Tune the // feedforward first, then the velocity feedback, then the outer // position PIDs above. Gains are in radians / second. @@ -159,11 +203,13 @@ async fn main(peripherals: Peripherals) { max_velocity: 0.0, }, ), + // TODO: set the starting pose (position in inches, heading) and the + // tracking-wheel geometry (wheel diameter and offset, in inches). WheeledTracking::new( (0.0, 0.0), - 90.0.deg(), - [TrackingWheel::new(forwards_enc, 2.0, 0.0, None)], - [TrackingWheel::new(sideways_enc, 2.0, 0.0, None)], + 0.0.deg(), + [TrackingWheel::new(forwards_enc, 0.0, 0.0, None)], + [TrackingWheel::new(sideways_enc, 0.0, 0.0, None)], Some(imu), ), ), diff --git a/src/motor_velocity.rs b/src/motor_velocity.rs new file mode 100644 index 0000000..088ab88 --- /dev/null +++ b/src/motor_velocity.rs @@ -0,0 +1,134 @@ +//! Background velocity tracking for a group of drive motors. +//! +//! [`MotorVelocityTracker`] owns one +//! [`VelocityEstimator`](crate::velocity_estimator::VelocityEstimator) per motor +//! and runs them from a background task, publishing the latest per-motor +//! output-shaft RPM into a shared cell that the drivetrain's velocity feedback +//! reads without touching the motors directly. +//! +//! Sharing the motors' `RefCell` with the drivetrain means the borrow to sample +//! positions must never be held across an `.await`, or it would collide with the +//! drivetrain's borrow to write voltages. +//! +//! # Sampling vs. consumption +//! +//! The estimator is sampled by this fixed-rate background task, so a controller +//! reading the shared cell may see a value up to one poll period old. This is +//! deliberate. The filter windows are sample-count based, so their time constants +//! are set by the poll rate; driving them from a control loop whose period varies +//! would make every time constant drift with loop load. The staleness is bounded +//! below the motor's data interval and is not significant. + +use std::{cell::RefCell, f64::consts::PI, rc::Rc}; + +use vexide::{ + math::Direction, + prelude::sleep, + smart::{ + motor::{Gearset, Motor}, + SmartDevice, + }, + task::Task, +}; + +use crate::{ + sensor::{TimestampedPosition, MOTOR_RAW_POSITION_RESPECTS_DIRECTION}, + velocity_estimator::VelocityEstimator, +}; + +/// Running sum and count of the live (`Some`) per-motor output-shaft RPM +/// readings, skipping failed reads so a `None` never drags the mean toward a +/// stale value. Shared by the drivetrain's velocity feedback and the sysid +/// collector so both average the group identically. +pub(crate) fn live_rpm_sum(velocities: &[Option]) -> (f64, usize) { + let mut sum = 0.0; + let mut count = 0; + for &rpm in velocities.iter().flatten() { + sum += rpm; + count += 1; + } + (sum, count) +} + +/// Converts a mean motor output-shaft RPM to wheel angular velocity (rad/s): +/// motor output RPM -> wheel RPM (via the external `gear_ratio`) -> rad/s. +pub(crate) fn wheel_omega_from_rpm(mean_rpm: f64, gear_ratio: f64) -> f64 { + mean_rpm * gear_ratio * (2.0 * PI / 60.0) +} + +/// Runs a [`VelocityEstimator`] per motor on a background task, exposing the +/// latest per-motor output-shaft RPM through a shared cell. An entry is `None` +/// while its motor's most recent read failed (e.g. unplugged), so consumers can +/// exclude it rather than average a stale value. +pub struct MotorVelocityTracker { + velocities: Rc>>>, + /// The tracking task, held so it's stopped when the tracker is dropped + /// rather than leaked via `detach()`. Never read directly. + _task: Task<()>, +} + +impl MotorVelocityTracker { + /// Spawns the tracking task over the shared `motors`. Sample order matches + /// the motor slice order. + /// + /// `gearset` is supplied by the caller rather than read from each motor, so a + /// motor that hasn't enumerated yet at power-on can't be silently mis-scaled + /// by a failed `gearset()` read. This assumes every motor in the group shares + /// the same gearset. + pub fn new(motors: Rc>>, gearset: Gearset) -> Self { + // One estimator per motor, all seeded with the caller-supplied output + // free speed for the shared gearset. + let gearset_rpm = gearset.max_rpm(); + let count = motors.borrow_mut().as_mut().len(); + let estimators: Vec<_> = (0..count) + .map(|_| VelocityEstimator::new(gearset_rpm)) + .collect(); + + let velocities = Rc::new(RefCell::new(vec![None; estimators.len()])); + + let task_motors = motors.clone(); + let task_velocities = velocities.clone(); + let task = vexide::task::spawn(async move { + let mut estimators = estimators; + loop { + // Poll at twice the ~10 ms publish rate: at exactly the publish + // rate, loop overhead would periodically read one packet twice + // and skip the next. Oversampling avoids that; the estimator's + // `dt == 0` early return discards the redundant reads for free. + sleep(Motor::UPDATE_INTERVAL / 2).await; + + // Borrow only for the synchronous update; drop everything before + // the next `.await` so the drivetrain can borrow to drive. + let mut motors = task_motors.borrow_mut(); + let mut results = task_velocities.borrow_mut(); + for (index, motor) in motors.as_mut().iter().enumerate() { + // A failed read publishes `None` so consumers drop this motor + // from the mean instead of averaging a stale value. + let Ok((ticks, timestamp)) = motor.timestamped_position() else { + results[index] = None; + continue; + }; + let mut rpm = estimators[index].update(ticks, timestamp); + if !MOTOR_RAW_POSITION_RESPECTS_DIRECTION + && matches!(motor.direction(), Ok(Direction::Reverse)) + { + rpm = -rpm; + } + results[index] = Some(rpm); + } + } + }); + + Self { + velocities, + _task: task, + } + } + + /// Runs `f` over the latest per-motor output-shaft RPM (`None` where the last + /// read failed) without cloning the shared `Rc` — for hot-path readers called + /// every control iteration. + pub fn with_velocities(&self, f: impl FnOnce(&[Option]) -> R) -> R { + f(&self.velocities.borrow()) + } +} diff --git a/src/sensor.rs b/src/sensor.rs new file mode 100644 index 0000000..1f30662 --- /dev/null +++ b/src/sensor.rs @@ -0,0 +1,123 @@ +//! Position/time sampling for the velocity estimator. +//! +//! The [`VelocityEstimator`](crate::velocity_estimator::VelocityEstimator) +//! differentiates raw encoder ticks against the Brain's clock. This module +//! defines the [`TimestampedPosition`] source that feeds it and implements it for +//! a V5 Smart [`Motor`]. + +use std::time::Duration; + +use vexide::{ + math::Direction, + prelude::sleep, + smart::{motor::Motor, PortError, SmartDevice}, + time::LowResolutionTime, +}; + +/// Whether `Motor::raw_position()` applies the motor's configured +/// [`Direction`](vexide::smart::motor::Direction) — i.e. reports the same sign as +/// `Motor::position()`. +/// +/// If `false`, callers must negate the estimator's output for motors configured +/// [`Direction::Reverse`](vexide::smart::motor::Direction::Reverse). +// TODO: verify on hardware with `probe_direction`. +pub const MOTOR_RAW_POSITION_RESPECTS_DIRECTION: bool = true; + +/// A source of a device's raw encoder position tagged with the Brain's clock +/// reading. +pub trait TimestampedPosition { + type Error; + /// Returns (raw encoder ticks, the Brain's clock reading in milliseconds). + fn timestamped_position(&self) -> Result<(i32, u32), Self::Error>; +} + +impl TimestampedPosition for Motor { + type Error = PortError; + + fn timestamped_position(&self) -> Result<(i32, u32), Self::Error> { + let ticks = self.raw_position()?; + + // `Motor::timestamp()` and the old `vexDeviceMotorPositionRawGet` out-param + // return the same value: `vexSystemTimeGet()` sampled when CPU1's V5_Device + // simpletask published this motor's packet. V5 motors transmit no timestamp of + // their own (their data packet carries only temperature, current, position, + // velocity, voltage, flags, faults), so the motor's actual sample time is not + // observable and may precede this by up to 10 ms. Both values refresh only + // during `vexTasksRun`, so these two reads always describe the same packet. + let timestamp = self + .timestamp()? + .duration_since(LowResolutionTime::EPOCH) + .as_millis() as u32; + + Ok((ticks, timestamp)) + } +} + +/// One-shot hardware diagnostic for [`MOTOR_RAW_POSITION_RESPECTS_DIRECTION`] and +/// `TICKS_PER_INTERNAL_REV`. +/// +/// Configures `motor` as [`Direction::Reverse`] — required, since under +/// [`Direction::Forward`] `raw_position()` and `position()` move the same way +/// regardless of whether the raw reading honors the flag — drives it at a low +/// positive voltage for ~500 ms, then compares the raw tick change against +/// `position()`, which definitively applies the flag. It prints whether the two +/// agree (the value the constant should hold) and the measured ticks per internal +/// revolution (expect ~50). The original direction is always restored before +/// returning. Run once against a free-spinning motor; nothing in the normal code +/// path calls this. +pub async fn probe_direction(motor: &mut Motor) -> Result<(), PortError> { + let original = motor.direction()?; + motor.set_direction(Direction::Reverse)?; + + // Measure with the flag set, then *always* restore the original direction — + // never leave the motor reconfigured, on any path. + let measured = drive_and_measure(motor).await; + motor.set_direction(original)?; + let (raw_delta, pos_delta) = measured?; + + // No motion on either reading means the motor stalled or is disconnected: + // nothing to compare, and `pos_delta == 0` would divide to inf/NaN below. + if raw_delta == 0 || pos_delta == 0.0 { + println!( + "probe_direction: no motion over 500 ms at +3 V (raw_delta = {raw_delta} ticks, \ + pos_delta = {pos_delta:.4} rev; motor stalled or disconnected?) — inconclusive." + ); + return Ok(()); + } + + // `position()` applies the direction flag; `raw_position()` honors it only if + // the two move the same way under Reverse. + let respects_direction = (raw_delta > 0) == (pos_delta > 0.0); + + // Ticks per revolution of the 3600 RPM internal rotor: raw ticks per *output* + // revolution divided by the gearset reduction (blue = 6.0). + let gearset_ratio = 3600.0 / motor.gearset()?.max_rpm(); + let ticks_per_internal_rev = (raw_delta as f64 / pos_delta).abs() / gearset_ratio; + + println!( + "probe_direction: raw_delta = {raw_delta} ticks, pos_delta = {pos_delta:.4} rev \ + over 500 ms at +3 V (Reverse). Set MOTOR_RAW_POSITION_RESPECTS_DIRECTION = \ + {respects_direction}; TICKS_PER_INTERNAL_REV ≈ {ticks_per_internal_rev:.1} (expect ~50)." + ); + Ok(()) +} + +/// Drives `motor` at +3 V for 500 ms and returns `(raw tick delta, output-shaft +/// revolution delta)`. Always stops the motor before returning, even if a read +/// fails, so the caller only has to restore the direction flag. +async fn drive_and_measure(motor: &mut Motor) -> Result<(i32, f64), PortError> { + let raw_start = motor.raw_position()?; + let pos_start = motor.position()?; + + motor.set_voltage(3.0)?; + sleep(Duration::from_millis(500)).await; + + // Read before stopping, but stop regardless of whether the reads succeeded. + let raw_end = motor.raw_position(); + let pos_end = motor.position(); + let _ = motor.set_voltage(0.0); + + let raw_delta = raw_end? - raw_start; + let pos_delta = (pos_end? - pos_start).as_turns(); + Ok((raw_delta, pos_delta)) +} diff --git a/src/sysid.rs b/src/sysid.rs index 171dcf4..a364ddf 100644 --- a/src/sysid.rs +++ b/src/sysid.rs @@ -27,7 +27,8 @@ //! //! ```text //! x_1=[time since step start] -//! y_1=[measured omega] +//! y_1=[filtered estimator omega] # fit this +//! z_1=[raw unfiltered omega] # plotted as a sanity check, not fitted //! ``` //! then in Desmos: `y_1 ~ a(1 - e^{-x_1/b})`, where `b` is the time constant //! `tau` and `Ka = Kv * tau` (from `tau = Ka/Kv`). Fit each step's block on @@ -39,11 +40,11 @@ //! straight into [`MotorFeedforward::new`](evian::control::loops::MotorFeedforward). //! //! ## Paste rules (Desmos silently breaks otherwise) -//! - Every list is on its own line and starts with `x_1=[` / `y_1=[` — copy one -//! whole line at a time; the `# ...` label lines are just guides, don't paste -//! them. -//! - Paste one block (its `x_1` and `y_1`) into a fresh Desmos before fitting; -//! the reused `x_1`/`y_1` names collide if you paste two blocks at once. +//! - Every list is on its own line and starts with `x_1=[`, `y_1=[`, or `z_1=[` +//! — copy one whole line at a time; the `# ...` label lines are just guides, +//! don't paste them. +//! - Paste one block's lists into a fresh Desmos before fitting; the reused +//! `x_1`/`y_1`/`z_1` names collide if you paste two blocks at once. //! - Numbers are fixed-decimal — Desmos can't read scientific notation like //! `1.2e-3` inside a list. //! @@ -56,11 +57,17 @@ //! enough that the robot fully coasts back to rest between steps. use std::{ - f64::consts::PI, + cell::RefCell, + rc::Rc, time::{Duration, Instant}, }; -use vexide::prelude::{sleep, Motor}; +use vexide::{ + prelude::{sleep, Motor}, + smart::motor::Gearset, +}; + +use crate::motor_velocity::{live_rpm_sum, wheel_omega_from_rpm, MotorVelocityTracker}; /// Fraction of each step's samples (from the end) averaged for the settled /// speed that feeds the steady-state fit. @@ -76,12 +83,17 @@ pub struct SysIdConfig { pub hold: Duration, /// How long to coast between steps so the robot returns to rest. pub rest: Duration, - /// Logging period. ≈10 ms gives the target ~100 Hz. + /// Logging period: how often a `(t, omega)` sample is buffered during each + /// step. This controls only the density of the printed data — it does *not* + /// affect any filter time constant. The estimator now runs in the shared + /// background [`MotorVelocityTracker`](crate::motor_velocity::MotorVelocityTracker) + /// at its own fixed poll rate, independent of this value, so changing it + /// can't invalidate the fit. 5 ms gives a dense rise for the transient fit. pub sample_interval: Duration, /// Wheel revolutions per motor output-shaft revolution — the *same* /// `gear_ratio` passed to `VelocityDifferential`, so the fitted constants /// land in the controller's wheel-rad/s units. `1.0` for direct drive. - /// ([`Motor::velocity`] already reports gearset-reduced RPM.) + /// (The estimator already reports gearset-reduced output-shaft RPM.) pub gear_ratio: f64, } @@ -91,18 +103,21 @@ impl Default for SysIdConfig { step_voltages: &[2.0, 3.0, 4.0, 5.0, 6.0, 7.0], hold: Duration::from_millis(1000), rest: Duration::from_millis(1500), - sample_interval: Duration::from_millis(10), + sample_interval: Duration::from_millis(5), gear_ratio: 1.0, } } } /// One staircase step: its label, the signed voltage held, and the buffered -/// `(t, omega)` samples of the rise. +/// `(t, estimated_omega, raw_omega)` samples of the rise. `estimated_omega` comes +/// from the per-side [`MotorVelocityTracker`] estimator pipeline; `raw_omega` is +/// the motor's own unfiltered velocity, kept alongside so the two can be compared +/// in Desmos. struct Step { label: String, volts: f64, - samples: Vec<(f64, f64)>, + samples: Vec<(f64, f64, f64)>, } /// Runs the full forward-then-reverse voltage staircase, then prints the @@ -112,7 +127,19 @@ struct Step { /// `left` and `right` are the two drive sides. They're commanded identically /// (equal voltage) so the robot tracks straight; the two sides are lumped into /// a single averaged `omega` measurement. -pub async fn collect(left: &mut [Motor], right: &mut [Motor], config: &SysIdConfig) { +pub async fn collect( + left: Rc>>, + right: Rc>>, + gearset: Gearset, + config: &SysIdConfig, +) { + // One background estimator per side, the same tracker the drivetrain uses. + // They run for the entire staircase — including the `rest` coasts between + // steps — so every step starts with warm filter windows instead of the + // empty ones a per-step estimator would give. + let left_tracker = MotorVelocityTracker::new(left.clone(), gearset); + let right_tracker = MotorVelocityTracker::new(right.clone(), gearset); + let mut steps = Vec::new(); // Interleave direction per level — run each level forward then immediately @@ -122,7 +149,7 @@ pub async fn collect(left: &mut [Motor], right: &mut [Motor], config: &SysIdConf for &level in config.step_voltages { for (phase, sign) in [("fwd", 1.0), ("rev", -1.0)] { let volts = sign * level; - let samples = run_step(left, right, volts, config).await; + let samples = run_step(&left, &right, &left_tracker, &right_tracker, volts, config).await; steps.push(Step { label: format!("{phase} {level:.1}V"), volts, @@ -130,33 +157,58 @@ pub async fn collect(left: &mut [Motor], right: &mut [Motor], config: &SysIdConf }); // Coast to a stop before the next step. 0 V = coast on a V5 motor. - set_all(left, right, 0.0); + set_all(left.borrow_mut().as_mut(), right.borrow_mut().as_mut(), 0.0); sleep(config.rest).await; } } // Belt and suspenders: make sure nothing is still driving before printing. - set_all(left, right, 0.0); + set_all(left.borrow_mut().as_mut(), right.borrow_mut().as_mut(), 0.0); print_desmos(&steps); } -/// Holds `volts` on every motor for `config.hold`, buffering a `(t, omega)` -/// sample every `config.sample_interval`. `t` is measured from the start of -/// this step. +/// Holds `volts` on every motor for `config.hold`, buffering a +/// `(t, estimated_omega, raw_omega)` sample every `config.sample_interval`. `t` +/// is measured from the start of this step. +/// +/// The estimated velocity is read from the persistent per-side +/// [`MotorVelocityTracker`]s, which run continuously across all steps, so each +/// step's filter windows are already warm when it begins. async fn run_step( - left: &mut [Motor], - right: &mut [Motor], + left: &Rc>>, + right: &Rc>>, + left_tracker: &MotorVelocityTracker, + right_tracker: &MotorVelocityTracker, volts: f64, config: &SysIdConfig, -) -> Vec<(f64, f64)> { +) -> Vec<(f64, f64, f64)> { let mut samples = Vec::new(); + let start = Instant::now(); while start.elapsed() < config.hold { - set_all(left, right, volts); - let omega = mean_omega(left, right, config.gear_ratio); + // Short synchronous borrows only — the tracker task borrows these same + // cells on its own schedule, so none may be held across the `.await`. + set_all(left.borrow_mut().as_mut(), right.borrow_mut().as_mut(), volts); + + // Lump both sides' live motors into one mean, excluding failed (`None`) + // reads, then convert exactly as `MotorGroupVelocity` does. + let (left_sum, left_count) = left_tracker.with_velocities(live_rpm_sum); + let (right_sum, right_count) = right_tracker.with_velocities(live_rpm_sum); + let count = left_count + right_count; + let estimated = if count == 0 { + 0.0 + } else { + wheel_omega_from_rpm((left_sum + right_sum) / count as f64, config.gear_ratio) + }; + + let raw = mean_raw_omega( + left.borrow_mut().as_mut(), + right.borrow_mut().as_mut(), + config.gear_ratio, + ); let t = start.elapsed().as_secs_f64(); - samples.push((t, omega)); + samples.push((t, estimated, raw)); sleep(config.sample_interval).await; } samples @@ -179,27 +231,39 @@ fn print_desmos(steps: &[Step]) { println!("y_1=[{}]", volts.join(",")); // --- Transient fits: one step at a time -> tau, then Ka = Kv * tau --- + // y_1 is the filtered estimator omega (fit this); z_1 is the raw unfiltered + // omega, plotted alongside as a sanity check on the estimator. for step in steps { println!(); println!( - "# transient {} -> tau=b, Ka=Kv*b (paste both lists, then: y_1 ~ a(1 - e^{{-x_1/b}}))", + "# transient {} -> tau=b, Ka=Kv*b (fit y_1 ~ a(1 - e^{{-x_1/b}}); z_1 is raw omega)", step.label ); - let ts: Vec = step.samples.iter().map(|(t, _)| format!("{t:.4}")).collect(); - let ws: Vec = step.samples.iter().map(|(_, w)| format!("{w:.4}")).collect(); + let ts: Vec = step.samples.iter().map(|(t, ..)| format!("{t:.4}")).collect(); + let ws: Vec = step + .samples + .iter() + .map(|(_, estimated, _)| format!("{estimated:.4}")) + .collect(); + let raws: Vec = step + .samples + .iter() + .map(|(.., raw)| format!("{raw:.4}")) + .collect(); println!("x_1=[{}]", ts.join(",")); println!("y_1=[{}]", ws.join(",")); + println!("z_1=[{}]", raws.join(",")); } } -/// Mean omega over the settled tail of a step's samples. -fn settled_omega(samples: &[(f64, f64)]) -> f64 { +/// Mean estimated omega over the settled tail of a step's samples. +fn settled_omega(samples: &[(f64, f64, f64)]) -> f64 { if samples.is_empty() { return 0.0; } let start = ((samples.len() as f64) * (1.0 - SETTLE_TAIL)) as usize; let tail = &samples[start.min(samples.len() - 1)..]; - tail.iter().map(|(_, w)| w).sum::() / tail.len() as f64 + tail.iter().map(|(_, estimated, _)| estimated).sum::() / tail.len() as f64 } /// Applies `volts` (clamped to each motor's range) to every drive motor. @@ -210,12 +274,12 @@ fn set_all(left: &mut [Motor], right: &mut [Motor], volts: f64) { } } -/// Mean wheel angular velocity (rad/s) across every drive motor, converting -/// motor-output RPM to wheel rad/s exactly as `MotorGroupVelocity` does. Motors -/// that error out are skipped. Because each motor's `Direction` is configured so -/// a positive command drives the robot forward, the readings are sign-consistent -/// with the commanded voltage and can be averaged directly. -fn mean_omega(left: &[Motor], right: &[Motor], gear_ratio: f64) -> f64 { +/// Mean wheel angular velocity (rad/s) from the motors' own *unfiltered* +/// [`Motor::velocity`], the pre-estimator baseline. Motors that error out are +/// skipped. Because each motor's `Direction` is configured so a positive command +/// drives the robot forward, the readings are sign-consistent with the commanded +/// voltage and can be averaged directly. +fn mean_raw_omega(left: &[Motor], right: &[Motor], gear_ratio: f64) -> f64 { let mut sum_rpm = 0.0; let mut count = 0.0; for motor in left.iter().chain(right.iter()) { @@ -227,5 +291,5 @@ fn mean_omega(left: &[Motor], right: &[Motor], gear_ratio: f64) -> f64 { if count == 0.0 { return 0.0; } - (sum_rpm / count) * gear_ratio * (2.0 * PI / 60.0) + wheel_omega_from_rpm(sum_rpm / count, gear_ratio) } diff --git a/src/velocity_differential.rs b/src/velocity_differential.rs index 9550390..f05ca0e 100644 --- a/src/velocity_differential.rs +++ b/src/velocity_differential.rs @@ -14,8 +14,9 @@ //! Each side's feedforward (`FF`) and feedback (`FB`) are independently optional; //! a missing half contributes `0.0` volts. The velocity feedback comes from a //! per-side [`WheelVelocity`] source (`S`); the built-in [`MotorGroupVelocity`] -//! reads the drive motors' own encoders, since `WheeledTracking` only reports -//! robot-frame velocity. +//! averages the drive motors' filtered velocities published by a +//! [`MotorVelocityTracker`], since `WheeledTracking` only reports robot-frame +//! velocity. //! //! The inner loop regulates each wheel's angular velocity in **radians / second** //! — hence a plain `Pid` over `f64` rather than an `AngularPid`, whose `±π` error @@ -24,7 +25,6 @@ use std::{ cell::RefCell, - f64::consts::PI, rc::Rc, time::{Duration, Instant}, }; @@ -34,7 +34,12 @@ use evian::{ drivetrain::model::{Arcade, DrivetrainModel}, math::desaturate, }; -use vexide::{prelude::Motor, smart::PortError}; +use vexide::{ + prelude::Motor, + smart::{motor::Gearset, PortError}, +}; + +use crate::motor_velocity::{live_rpm_sum, wheel_omega_from_rpm, MotorVelocityTracker}; /// A source of a drivetrain side's measured wheel angular velocity, in /// radians / second. @@ -42,41 +47,37 @@ pub trait WheelVelocity { fn velocity(&mut self) -> f64; } -/// A [`WheelVelocity`] source backed by a group of drive motors' internal -/// encoders, sharing ownership of the motors with the drivetrain so they can be -/// both driven and read. +/// A [`WheelVelocity`] source backed by a [`MotorVelocityTracker`], averaging the +/// group's filtered output-shaft velocities. pub struct MotorGroupVelocity { - motors: Rc>>, + /// Owns the background tracker so its task lives exactly as long as this + /// source (and the drivetrain that holds it). + tracker: MotorVelocityTracker, /// Wheel revolutions per motor output-shaft revolution (external gearing - /// only; [`Motor::velocity`] already reports gearset-reduced RPM). `1.0` for - /// direct drive. + /// only; the tracker already reports gearset-reduced RPM). `1.0` for direct + /// drive. gear_ratio: f64, } impl MotorGroupVelocity { - pub fn new(motors: Rc>>, gear_ratio: f64) -> Self { - Self { motors, gear_ratio } + pub fn new(tracker: MotorVelocityTracker, gear_ratio: f64) -> Self { + Self { + tracker, + gear_ratio, + } } } impl WheelVelocity for MotorGroupVelocity { fn velocity(&mut self) -> f64 { - let mut borrow = self.motors.borrow_mut(); - let motors = borrow.as_mut(); - - let mut sum_rpm = 0.0; - let mut count = 0.0; - for motor in motors.iter() { - if let Ok(rpm) = motor.velocity() { - sum_rpm += rpm; - count += 1.0; + let gear_ratio = self.gear_ratio; + self.tracker.with_velocities(|velocities| { + let (sum_rpm, count) = live_rpm_sum(velocities); + if count == 0 { + return 0.0; } - } - if count == 0.0 { - return 0.0; - } - // motor output RPM -> wheel RPM -> wheel rad/s - (sum_rpm / count) * self.gear_ratio * (2.0 * PI / 60.0) + wheel_omega_from_rpm(sum_rpm / count as f64, gear_ratio) + }) } } @@ -113,22 +114,22 @@ pub struct VelocityDifferential { impl VelocityDifferential { /// Reads its velocity feedback from the drive motors' own encoders. - /// `gear_ratio` configures the built-in [`MotorGroupVelocity`] sources; for a + /// `gear_ratio` configures the built-in [`MotorGroupVelocity`] sources and + /// `gearset` seeds their estimators (shared across every motor); for a /// custom velocity source, use [`with_sources`](Self::with_sources) instead. - pub fn new( - left: L, - right: R, + pub fn new( + left: Rc>>, + right: Rc>>, gear_ratio: f64, + gearset: Gearset, config: VelocityDifferentialConfig, - ) -> Self - where - L: AsMut<[Motor]> + 'static, - R: AsMut<[Motor]> + 'static, - { - let left: Rc>> = Rc::new(RefCell::new(left)); - let right: Rc>> = Rc::new(RefCell::new(right)); - let left_source = MotorGroupVelocity::new(left.clone(), gear_ratio); - let right_source = MotorGroupVelocity::new(right.clone(), gear_ratio); + ) -> Self { + // Each side gets its own background estimator, owned by its source so the + // task runs exactly as long as the drivetrain holds the source. + let left_source = + MotorGroupVelocity::new(MotorVelocityTracker::new(left.clone(), gearset), gear_ratio); + let right_source = + MotorGroupVelocity::new(MotorVelocityTracker::new(right.clone(), gearset), gear_ratio); Self { left, @@ -147,20 +148,16 @@ impl VelocityDifferential { /// Uses custom per-side [`WheelVelocity`] feedback sources. // Deliberate public plug-point; not exercised by the default wiring. #[allow(dead_code)] - pub fn with_sources( - left: L, - right: R, + pub fn with_sources( + left: Rc>>, + right: Rc>>, left_source: S, right_source: S, config: VelocityDifferentialConfig, - ) -> Self - where - L: AsMut<[Motor]> + 'static, - R: AsMut<[Motor]> + 'static, - { + ) -> Self { Self { - left: Rc::new(RefCell::new(left)), - right: Rc::new(RefCell::new(right)), + left, + right, left_source, right_source, config, @@ -247,9 +244,8 @@ where (0.0, 0.0) }; - // Read the velocity feedback *before* borrowing the motors for writing: - // the default source shares the same motor `RefCell`, so the read and - // the write must not overlap. + // Current per-side velocity feedback, a cheap read of the trackers' + // shared cells (does not touch the motors). let left_measured = self.left_source.velocity(); let right_measured = self.right_source.velocity(); diff --git a/src/velocity_estimator.rs b/src/velocity_estimator.rs new file mode 100644 index 0000000..3bd72f2 --- /dev/null +++ b/src/velocity_estimator.rs @@ -0,0 +1,276 @@ +//! A sylib-style motor velocity estimator. +//! +//! V5 Smart motors report an internally-estimated velocity ([`Motor::velocity`]), +//! but that estimate is noisy and laggy at the speeds a drivetrain velocity loop +//! cares about. This estimator instead differentiates the motor's raw encoder +//! position against the Brain's clock and runs the result through a small +//! filter chain, closely following sylib's approach: +//! +//! +//! The filter chain follows sylib; the dT handling deliberately does not. sylib +//! corrects dt using a per-sample motor timestamp, quantizing it to 5 ms +//! multiples to undo the motor's ~5 ms sampling straddle. That correction is +//! omitted here: V5 motors transmit no timestamp of their own, so the straddle +//! it compensates for is not observable from the Brain's clock. Re-adding it +//! would only distort dt whenever a packet publish is missed. (This has been +//! mistakenly re-added once — leave it out.) +//! +//! The pipeline, per [`update`](VelocityEstimator::update): +//! +//! 1. A raw RPM from the tick/time difference (at the motor's *internal* shaft). +//! 2. A 3-tap moving average to knock down encoder quantization noise. +//! 3. A 7-tap median off the smoothed value to reject single-sample spikes. +//! 4. A derivative of the median (an acceleration estimate) whose recent peak +//! magnitude drives an adaptive EMA gain: the filter tracks quickly during +//! acceleration transients and smooths hard when the speed is steady. +//! +//! All time in this file is in **milliseconds**. The adaptive-gain constants are +//! calibrated to that scale, so do not convert to seconds anywhere here. +//! +//! # Startup +//! +//! The filters operate on partial windows, so the first ~20 samples are +//! progressively smoothed rather than fully filtered. This is intentional and +//! matches sylib, which divides by the actual sample count. The alternative — +//! reporting zero until every window fills — would emit ~200 ms of false zero +//! velocity that a feedback loop would integrate as real error. +//! +//! This module has no `vexide`/hardware dependency and is unit-tested on the +//! host; the caller supplies `(ticks, timestamp_ms)` (see +//! [`TimestampedPosition`](crate::sensor::TimestampedPosition)). + +use crate::filters::{Derivative, Ema, MaxAbs, Median, Sma}; + +/// Raw encoder ticks per revolution of the motor's *internal* (pre-gearset) +/// shaft. +// +// 50 ticks per revolution of the 3600 RPM internal rotor. Confirmed by VEX's +// published per-output-revolution figures (1800 at 36:1, 900 at 18:1, 300 at +// 6:1), which are also vexide's `Gearset::*_TICKS_PER_REVOLUTION` constants — +// each divided by its reduction gives 50. Note that vexide's `raw_position` doc +// comment claims a TPR of 4096; that contradicts vexide's own gearset constants +// and is wrong. +const TICKS_PER_INTERNAL_REV: f64 = 50.0; + +/// Free speed of the motor's internal shaft, in RPM. Output-shaft RPM is +/// internal RPM scaled by `GEARSET_RPM / INTERNAL_FREE_SPEED_RPM`. +const INTERNAL_FREE_SPEED_RPM: f64 = 3600.0; + +/// Any raw RPM whose magnitude exceeds this is treated as a spurious position +/// reset (e.g. `raw_position()` being re-zeroed) rather than real motion. +const MAX_PLAUSIBLE_RAW_RPM: f64 = 5000.0; + +/// EMA gain approached under hard acceleration. +const GAIN_MAX: f64 = 0.75; + +/// EMA gain at zero acceleration — heavy steady-state smoothing. +/// +/// At the estimator's ~10 ms effective sample rate this is a time constant near +/// one second. +/// +// TODO: this is probably too low for a drivetrain velocity loop. These constants +// come from sylib, where they were tuned empirically for flywheels — a plant +// that is slow anyway and cares far more about steady-state accuracy than +// latency. A drivetrain velocity loop is the opposite trade: ~1 s of lag in the +// feedback path forces the velocity PID's gains down far enough to partly defeat +// having the loop at all. +// +// The adaptive gain rescues large transients, which is its purpose. It does not +// rescue small disturbances near steady state, where `peak` stays low, the gain +// sits at this floor, and the loop reacts to information up to a second stale. +// +// Check this against the sysid traces: if `y_1` visibly lags into the flat +// region while `z_1` has already settled, the floor is too low. First thing to +// try is raising this to ~0.05 (a ~200 ms time constant) and re-running. +const GAIN_MIN: f64 = 0.0096; + +/// Knee of the gain curve, in (RPM/ms)². The gain sits midway between +/// [`GAIN_MIN`] and [`GAIN_MAX`] when peak acceleration is `sqrt` of this +/// (≈ 7.1 RPM/ms, about 1180 output RPM/s on a blue cartridge). +/// +/// Also from sylib, also empirical — there is no derivation behind it, and this +/// drivetrain has no particular reason to want the same knee. +const ACCEL_SCALE: f64 = 50.0; + +/// Estimates a single motor's output-shaft velocity from timestamped raw +/// encoder samples. +pub struct VelocityEstimator { + /// Motor free speed at the output shaft for this motor's gearset, in RPM + /// (e.g. 600 for a blue cartridge). + gearset_rpm: f64, + + sma_3: Sma, + median_7: Median, + derivative: Derivative, + max_abs_20: MaxAbs, + ema: Ema, + + previous_ticks: i32, + previous_timestamp_ms: u32, + last_output: f64, + seeded: bool, +} + +impl VelocityEstimator { + /// `gearset_rpm` is the motor's output-shaft free speed for its gearset + /// (blue = 600). + pub fn new(gearset_rpm: f64) -> Self { + Self { + gearset_rpm, + sma_3: Sma::new(3), + median_7: Median::new(7), + derivative: Derivative::new(), + max_abs_20: MaxAbs::new(20), + ema: Ema::new(), + previous_ticks: 0, + previous_timestamp_ms: 0, + last_output: 0.0, + seeded: false, + } + } + + /// Feeds one timestamped raw-encoder sample and returns the estimated motor + /// output-shaft velocity in RPM. + /// + /// `ticks` is the raw (pre-gearset) encoder count and `timestamp_ms` is the + /// Brain's clock reading in milliseconds. + pub fn update(&mut self, ticks: i32, timestamp_ms: u32) -> f64 { + // The first sample only establishes a baseline to difference against. + if !self.seeded { + self.previous_ticks = ticks; + self.previous_timestamp_ms = timestamp_ms; + self.seeded = true; + return self.last_output; + } + + // 1. dt from the Brain's clock. No elapsed time -> nothing new to say. + let dt = timestamp_ms.wrapping_sub(self.previous_timestamp_ms); + if dt == 0 { + return self.last_output; + } + let dt = dt as f64; + + // 2. Raw internal-shaft RPM: revs over the interval, scaled to per-minute. + let delta_ticks = (ticks as i64 - self.previous_ticks as i64) as f64; + + // Advance the baseline *before* the reset guard below can bail out. + // Otherwise a real `reset_position()` wedges the estimator forever: every + // later sample would difference against the stale pre-reset ticks, so + // `raw_rpm` would stay above the threshold and we'd never output again. + self.previous_ticks = ticks; + self.previous_timestamp_ms = timestamp_ms; + + let raw_rpm = (delta_ticks / TICKS_PER_INTERNAL_REV) / dt * 60_000.0; + + // 3. An implausible jump means the encoder was reset, not that the motor + // briefly hit thousands of RPM: drop the sample. The baseline was + // already advanced above, so the next sample differences against this + // one; only the filters and `last_output` are left untouched. + if raw_rpm.abs() > MAX_PLAUSIBLE_RAW_RPM { + return self.last_output; + } + + // 4. Smooth the quantization noise. + let smoothed = self.sma_3.filter(raw_rpm); + // 5. Reject single-sample spikes for the acceleration estimate. + let median = self.median_7.filter(smoothed); + // 6. Acceleration estimate (RPM per millisecond)... + let accel = self.derivative.filter(median, dt); + // 7. ...and its recent peak magnitude. + let peak = self.max_abs_20.filter(accel); + // 8. Adaptive gain: a saturating curve from GAIN_MIN at rest toward + // GAIN_MAX under acceleration, so the estimate tracks quickly through + // transients and smooths hard when the speed is steady. `shape` is the + // constant that places the floor at GAIN_MIN; it is derived from the + // other two rather than tuned. See GAIN_MIN's note on the floor being + // likely too low for a drivetrain. + let shape = GAIN_MAX / (GAIN_MAX - GAIN_MIN); + let gain = GAIN_MAX * (1.0 - 1.0 / ((peak * peak / ACCEL_SCALE) + shape)); + // 9. EMA the *smoothed* value with the adaptive gain, then convert + // internal-shaft RPM to output-shaft RPM. + let output = + self.ema.filter(smoothed, gain) * self.gearset_rpm / INTERNAL_FREE_SPEED_RPM; + + self.last_output = output; + output + } +} + +#[cfg(test)] +mod tests { + use super::*; + + /// Blue-cartridge free speed used across the tests. + const BLUE_RPM: f64 = 600.0; + + #[test] + fn first_sample_only_seeds_and_returns_zero() { + let mut estimator = VelocityEstimator::new(BLUE_RPM); + assert_eq!(estimator.update(0, 100), 0.0); + } + + #[test] + fn zero_dt_returns_last_output_unchanged() { + let mut estimator = VelocityEstimator::new(BLUE_RPM); + estimator.update(0, 100); + let a = estimator.update(50, 110); + // Same timestamp -> dt == 0 -> unchanged. + let b = estimator.update(999, 110); + assert_eq!(a, b); + } + + #[test] + fn position_reset_spike_is_ignored() { + let mut estimator = VelocityEstimator::new(BLUE_RPM); + estimator.update(0, 0); + // 30 ticks / 10 ms = 3600 internal RPM: plausible motion. + let before = estimator.update(30, 10); + // A huge tick jump over a short interval is an encoder reset, not motion: + // the sample is dropped and the last output held. + let after = estimator.update(1_000_000, 20); + assert_eq!(before, after); + + // Because the baseline advanced to the reset value, normal motion resumes + // immediately: the next samples difference against 1_000_000, not the + // stale pre-reset ticks, so the estimator recovers instead of wedging. + let recovered = estimator.update(1_000_030, 30); + let recovered = estimator.update(1_000_060, 40).max(recovered); + assert!( + recovered > 1.0, + "estimator should recover after a reset, got {recovered}" + ); + } + + #[test] + fn steady_state_converges_to_expected_output_rpm() { + // Spin the internal shaft at a constant 30 ticks / 10 ms. + // 30 ticks / 50 ticks-per-rev = 0.6 rev per 10 ms = 3600 internal RPM, + // which at the blue gearset is 3600 * 600 / 3600 = 600 output RPM. The + // steady-state gain is small, so run long enough for the EMA to settle. + let mut estimator = VelocityEstimator::new(BLUE_RPM); + let mut ticks = 0i32; + let mut t = 0u32; + let mut output = 0.0; + for _ in 0..3000 { + ticks += 30; + t += 10; + output = estimator.update(ticks, t); + } + assert!( + (output - 600.0).abs() < 1.0, + "expected ~600 output RPM, got {output}" + ); + } + + #[test] + fn output_is_zero_when_stationary() { + let mut estimator = VelocityEstimator::new(BLUE_RPM); + let mut output = 0.0; + for t in (0..500).step_by(10) { + output = estimator.update(0, t as u32); + } + assert!(output.abs() < EPS, "expected ~0 output RPM, got {output}"); + } + + const EPS: f64 = 1e-9; +}