From 31b8844a2b118282b7345275c2b381cab6a6f78c Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Mon, 7 Sep 2026 19:58:06 -0700 Subject: [PATCH 01/18] Add sylib-style motor velocity estimator for drivetrain feedback Replace Motor::velocity() as the drivetrain's velocity source with a filtered estimator that differentiates raw encoder ticks against the motor's own clock, following sylib's approach. - filters.rs: Sma, Median, MaxAbs, Ema, and Derivative scalar filters, vexide-free and unit-tested. - velocity_estimator.rs: VelocityEstimator running the SMA -> median -> derivative -> max-abs adaptive-EMA-gain pipeline, returning output-shaft RPM. Also vexide-free and unit-tested. - sensor.rs: TimestampedPosition trait (raw ticks + device-clock ms), implemented for Motor via raw_position() and SmartDevice::timestamp(). - motor_velocity.rs: MotorVelocityTracker runs one estimator per motor on a background task, publishing per-motor RPM into a shared cell without holding the motor borrow across an await. - velocity_differential.rs: MotorGroupVelocity now averages the tracker's shared cell instead of polling motor.velocity(); VelocityDifferential wires a per-side tracker into each source. - sysid.rs: records both estimated and raw omega per sample and emits the raw series as a third Desmos list (z_1) for comparison. Co-Authored-By: Claude Opus 4.8 --- src/filters.rs | 235 +++++++++++++++++++++++++++++++++++ src/main.rs | 5 + src/motor_velocity.rs | 90 ++++++++++++++ src/sensor.rs | 46 +++++++ src/sysid.rs | 125 +++++++++++++++---- src/velocity_differential.rs | 60 ++++----- src/velocity_estimator.rs | 200 +++++++++++++++++++++++++++++ 7 files changed, 712 insertions(+), 49 deletions(-) create mode 100644 src/filters.rs create mode 100644 src/motor_velocity.rs create mode 100644 src/sensor.rs create mode 100644 src/velocity_estimator.rs diff --git a/src/filters.rs b/src/filters.rs new file mode 100644 index 0000000..13352a4 --- /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 lower 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..0c2f20b 100644 --- a/src/main.rs +++ b/src/main.rs @@ -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}; diff --git a/src/motor_velocity.rs b/src/motor_velocity.rs new file mode 100644 index 0000000..71d2fcd --- /dev/null +++ b/src/motor_velocity.rs @@ -0,0 +1,90 @@ +//! 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 consumers (the drivetrain's velocity +//! feedback and the sysid collector) read 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. + +use std::{cell::RefCell, rc::Rc}; + +use vexide::{ + math::Direction, + prelude::sleep, + smart::{motor::Motor, SmartDevice}, +}; + +use crate::{ + sensor::{TimestampedPosition, MOTOR_RAW_POSITION_RESPECTS_DIRECTION}, + velocity_estimator::VelocityEstimator, +}; + +/// Fallback output-shaft free speed (blue cartridge) used if a motor's gearset +/// can't be read while building its estimator. +const DEFAULT_GEARSET_RPM: f64 = 600.0; + +/// Runs a [`VelocityEstimator`] per motor on a background task, exposing the +/// latest per-motor output-shaft RPM through a shared cell. +pub struct MotorVelocityTracker { + velocities: Rc>>, +} + +impl MotorVelocityTracker { + /// Spawns the tracking task over the shared `motors`. Sample order matches + /// the motor slice order. + pub fn new(motors: Rc>>) -> Self { + // Build one estimator per motor, seeding each with its own gearset speed. + let mut estimators = Vec::new(); + { + let mut borrow = motors.borrow_mut(); + for motor in borrow.as_mut().iter() { + let gearset_rpm = motor + .gearset() + .map(|gearset| gearset.max_rpm()) + .unwrap_or(DEFAULT_GEARSET_RPM); + estimators.push(VelocityEstimator::new(gearset_rpm)); + } + } + + let velocities = Rc::new(RefCell::new(vec![0.0; estimators.len()])); + + let task_motors = motors.clone(); + let task_velocities = velocities.clone(); + vexide::task::spawn(async move { + let mut estimators = estimators; + loop { + sleep(Motor::UPDATE_INTERVAL).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() { + let Ok((ticks, timestamp)) = motor.timestamped_position() else { + 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] = rpm; + } + } + }) + .detach(); + + Self { velocities } + } + + /// A shared handle to the latest per-motor output-shaft RPM, updated in + /// place by the background task. + pub fn velocities(&self) -> Rc>> { + self.velocities.clone() + } +} diff --git a/src/sensor.rs b/src/sensor.rs new file mode 100644 index 0000000..6f400ec --- /dev/null +++ b/src/sensor.rs @@ -0,0 +1,46 @@ +//! Position/time sampling for the velocity estimator. +//! +//! The [`VelocityEstimator`](crate::velocity_estimator::VelocityEstimator) +//! differentiates raw encoder ticks against the device's own clock. This module +//! defines the [`TimestampedPosition`] source that feeds it and implements it for +//! a V5 Smart [`Motor`]. + +use vexide::{ + smart::{motor::Motor, PortError, SmartDevice}, + time::LowResolutionTime, +}; + +/// Whether `Motor::raw_position()` already accounts for the motor's configured +/// [`Direction`](vexide::smart::motor::Direction) (so a reversed motor reads +/// negative ticks when driven "forward"). +/// +/// If this is `false`, callers must negate the estimator's output for motors +/// configured [`Direction::Reverse`](vexide::smart::motor::Direction::Reverse). +// TODO: verify on hardware. +pub const MOTOR_RAW_POSITION_RESPECTS_DIRECTION: bool = true; + +/// A source of a device's raw encoder position tagged with the device's own +/// clock reading, both sampled as close together as the API allows. +pub trait TimestampedPosition { + type Error; + /// Returns (raw encoder ticks, device 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()?; + + // TODO: this is the Brain's packet-processed timestamp, not the motor's own record + // of when it sampled, and the two reads below may describe different samples. Swap + // to the vexDeviceMotorPositionRawGet out-param once vexide exposes it (vexide#386). + let timestamp = self + .timestamp()? + .duration_since(LowResolutionTime::EPOCH) + .as_millis() as u32; + + Ok((ticks, timestamp)) + } +} diff --git a/src/sysid.rs b/src/sysid.rs index 171dcf4..dbb2ac3 100644 --- a/src/sysid.rs +++ b/src/sysid.rs @@ -60,7 +60,19 @@ use std::{ time::{Duration, Instant}, }; -use vexide::prelude::{sleep, Motor}; +use vexide::{ + math::Direction, + prelude::{sleep, Motor}, +}; + +use crate::{ + sensor::{TimestampedPosition, MOTOR_RAW_POSITION_RESPECTS_DIRECTION}, + velocity_estimator::VelocityEstimator, +}; + +/// Fallback output-shaft free speed (blue cartridge) if a motor's gearset can't +/// be read while building its estimator. +const DEFAULT_GEARSET_RPM: f64 = 600.0; /// Fraction of each step's samples (from the end) averaged for the settled /// speed that feeds the steady-state fit. @@ -98,11 +110,13 @@ impl Default for SysIdConfig { } /// 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 [`VelocityEstimator`] 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 @@ -141,27 +155,73 @@ pub async fn collect(left: &mut [Motor], right: &mut [Motor], config: &SysIdConf 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. +/// +/// A fresh [`VelocityEstimator`] per motor is built for each step so the filter +/// state doesn't carry across the coast between steps. async fn run_step( left: &mut [Motor], right: &mut [Motor], volts: f64, config: &SysIdConfig, -) -> Vec<(f64, f64)> { +) -> Vec<(f64, f64, f64)> { let mut samples = Vec::new(); + let mut estimators = build_estimators(left, right); + let mut velocities = vec![0.0; estimators.len()]; + let start = Instant::now(); while start.elapsed() < config.hold { set_all(left, right, volts); - let omega = mean_omega(left, right, config.gear_ratio); + update_estimators(left, right, &mut estimators, &mut velocities); + let estimated = mean_omega(&velocities, config.gear_ratio); + let raw = mean_raw_omega(left, right, 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 } +/// One [`VelocityEstimator`] per drive motor (left then right), each seeded with +/// its motor's gearset free speed. +fn build_estimators(left: &[Motor], right: &[Motor]) -> Vec { + left.iter() + .chain(right.iter()) + .map(|motor| { + let gearset_rpm = motor + .gearset() + .map(|gearset| gearset.max_rpm()) + .unwrap_or(DEFAULT_GEARSET_RPM); + VelocityEstimator::new(gearset_rpm) + }) + .collect() +} + +/// Feeds one timestamped sample into every estimator, writing the resulting +/// per-motor output-shaft RPM into `velocities` (left then right). Motors that +/// error out keep their previous value. +fn update_estimators( + left: &[Motor], + right: &[Motor], + estimators: &mut [VelocityEstimator], + velocities: &mut [f64], +) { + for (index, motor) in left.iter().chain(right.iter()).enumerate() { + let Ok((ticks, timestamp)) = motor.timestamped_position() else { + continue; + }; + let mut rpm = estimators[index].update(ticks, timestamp); + if !MOTOR_RAW_POSITION_RESPECTS_DIRECTION + && matches!(motor.direction(), Ok(Direction::Reverse)) + { + rpm = -rpm; + } + velocities[index] = rpm; + } +} + /// Prints the collected steps as Desmos list literals: one steady-state block /// for `Ks`/`Kv`, then one transient block per step for `Ka`. Each list is /// alone on its own line with a fixed-decimal, scientific-notation-free format @@ -179,27 +239,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 +282,23 @@ 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 estimator pipeline's per-motor +/// output-shaft RPM in `velocities`, converting to wheel rad/s exactly as +/// `MotorGroupVelocity` does. +fn mean_omega(velocities: &[f64], gear_ratio: f64) -> f64 { + if velocities.is_empty() { + return 0.0; + } + let mean_rpm = velocities.iter().sum::() / velocities.len() as f64; + mean_rpm * gear_ratio * (2.0 * PI / 60.0) +} + +/// 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()) { diff --git a/src/velocity_differential.rs b/src/velocity_differential.rs index 9550390..1f32a90 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 @@ -36,47 +37,43 @@ use evian::{ }; use vexide::{prelude::Motor, smart::PortError}; +use crate::motor_velocity::MotorVelocityTracker; + /// A source of a drivetrain side's measured wheel angular velocity, in /// radians / second. 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`]'s shared +/// per-motor RPM cell, averaging the group's filtered output-shaft velocities. pub struct MotorGroupVelocity { - motors: Rc>>, + /// Latest per-motor output-shaft RPM, published by the background tracker. + velocities: Rc>>, /// 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(velocities: Rc>>, gear_ratio: f64) -> Self { + Self { + velocities, + 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; - } - } - if count == 0.0 { + let velocities = self.velocities.borrow(); + if velocities.is_empty() { return 0.0; } + let mean_rpm = velocities.iter().sum::() / velocities.len() as f64; // motor output RPM -> wheel RPM -> wheel rad/s - (sum_rpm / count) * self.gear_ratio * (2.0 * PI / 60.0) + mean_rpm * self.gear_ratio * (2.0 * PI / 60.0) } } @@ -127,8 +124,16 @@ impl VelocityDifferential { { 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); + + // Each side gets its own background estimator; the trackers can be + // dropped here because their detached tasks keep the shared velocity + // cells alive alongside the sources that read them. + let left_source = + MotorGroupVelocity::new(MotorVelocityTracker::new(left.clone()).velocities(), gear_ratio); + let right_source = MotorGroupVelocity::new( + MotorVelocityTracker::new(right.clone()).velocities(), + gear_ratio, + ); Self { left, @@ -247,9 +252,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. + // Read the velocity feedback from the trackers' shared cells before + // borrowing the motors for writing. 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..bdbe2d7 --- /dev/null +++ b/src/velocity_estimator.rs @@ -0,0 +1,200 @@ +//! 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 motor's own clock and runs the result through a small +//! filter chain, closely following sylib's approach: +//! +//! +//! 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 gain constants in step 8 +//! are calibrated to that scale, so do not convert to seconds anywhere here. +//! +//! 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. +// TODO: verify on hardware by spinning one full output revolution and diffing +// raw_position(). +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; + +/// 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 + /// motor's own 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 motor's own 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; + 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 without disturbing + // the filters or the baseline. + 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: near 0 when steady, rising toward 0.75 during + // acceleration transients so the estimate keeps up. + let gain = 0.75 * (1.0 - 1.0 / ((peak * peak / 50.0) + 1.013)); + // 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.previous_ticks = ticks; + self.previous_timestamp_ms = timestamp_ms; + 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. + let after = estimator.update(1_000_000, 20); + assert_eq!(before, after); + } + + #[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; +} From ebe2f5ac4f1b38d978561393284c75c12c3ec5e1 Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Mon, 7 Sep 2026 20:16:17 -0700 Subject: [PATCH 02/18] Snap estimator dt to the 5ms motor grid; fix timestamp comment Motor::timestamp() reports the motor's own sample clock, so the sensor comment no longer flags it as the Brain's packet timestamp. Snapping dt to the nearest 5ms (the motor's sampling grid) removes the millisecond-scale timestamp jitter that otherwise shows up as velocity quantization noise at constant speed. Reads closer than 5ms apart round to a 0ms dt and are treated as no new sample. Co-Authored-By: Claude Opus 4.8 --- src/sensor.rs | 6 +++--- src/velocity_estimator.rs | 25 ++++++++++++++++++++----- 2 files changed, 23 insertions(+), 8 deletions(-) diff --git a/src/sensor.rs b/src/sensor.rs index 6f400ec..4948d4f 100644 --- a/src/sensor.rs +++ b/src/sensor.rs @@ -33,9 +33,9 @@ impl TimestampedPosition for Motor { fn timestamped_position(&self) -> Result<(i32, u32), Self::Error> { let ticks = self.raw_position()?; - // TODO: this is the Brain's packet-processed timestamp, not the motor's own record - // of when it sampled, and the two reads below may describe different samples. Swap - // to the vexDeviceMotorPositionRawGet out-param once vexide exposes it (vexide#386). + // Motor::timestamp() reports the motor's own sample clock (equivalent to + // the vexDeviceMotorPositionRawGet out-param), so it pairs with the raw + // position read above. let timestamp = self .timestamp()? .duration_since(LowResolutionTime::EPOCH) diff --git a/src/velocity_estimator.rs b/src/velocity_estimator.rs index bdbe2d7..207b58a 100644 --- a/src/velocity_estimator.rs +++ b/src/velocity_estimator.rs @@ -9,7 +9,9 @@ //! //! The pipeline, per [`update`](VelocityEstimator::update): //! -//! 1. A raw RPM from the tick/time difference (at the motor's *internal* shaft). +//! 1. A raw RPM from the tick/time difference (at the motor's *internal* shaft), +//! with `dt` snapped to the motor's 5 ms sampling grid to reject timestamp +//! jitter. //! 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 @@ -90,12 +92,15 @@ impl VelocityEstimator { return self.last_output; } - // 1. dt from the motor's own clock. No elapsed time -> nothing new to say. - let dt = timestamp_ms.wrapping_sub(self.previous_timestamp_ms); - if dt == 0 { + // 1. dt from the motor's own clock, snapped to the motor's 5 ms sampling + // grid. The reported timestamps jitter by a millisecond or two, and + // that jitter would otherwise turn a constant speed into velocity + // quantization noise; rounding to the grid removes it permanently. + // Reads closer together than 5 ms round to 0 -> no new sample yet. + let dt = 5.0 * (timestamp_ms.wrapping_sub(self.previous_timestamp_ms) as f64 / 5.0).round(); + if dt == 0.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; @@ -154,6 +159,16 @@ mod tests { assert_eq!(a, b); } + #[test] + fn sub_grid_reads_snap_to_no_new_sample() { + let mut estimator = VelocityEstimator::new(BLUE_RPM); + estimator.update(0, 0); + let a = estimator.update(30, 10); + // Only 2 ms later: rounds to a 0 ms dt, so nothing changes. + let b = estimator.update(45, 12); + assert_eq!(a, b); + } + #[test] fn position_reset_spike_is_ignored() { let mut estimator = VelocityEstimator::new(BLUE_RPM); From 27e0a20e022bc6bb1f9ccb7b5e70d771cd91e1f5 Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Mon, 7 Sep 2026 23:47:35 -0700 Subject: [PATCH 03/18] Revert "Snap estimator dt to the 5ms motor grid; fix timestamp comment" This reverts commit ebe2f5ac4f1b38d978561393284c75c12c3ec5e1. --- src/sensor.rs | 6 +++--- src/velocity_estimator.rs | 25 +++++-------------------- 2 files changed, 8 insertions(+), 23 deletions(-) diff --git a/src/sensor.rs b/src/sensor.rs index 4948d4f..6f400ec 100644 --- a/src/sensor.rs +++ b/src/sensor.rs @@ -33,9 +33,9 @@ impl TimestampedPosition for Motor { fn timestamped_position(&self) -> Result<(i32, u32), Self::Error> { let ticks = self.raw_position()?; - // Motor::timestamp() reports the motor's own sample clock (equivalent to - // the vexDeviceMotorPositionRawGet out-param), so it pairs with the raw - // position read above. + // TODO: this is the Brain's packet-processed timestamp, not the motor's own record + // of when it sampled, and the two reads below may describe different samples. Swap + // to the vexDeviceMotorPositionRawGet out-param once vexide exposes it (vexide#386). let timestamp = self .timestamp()? .duration_since(LowResolutionTime::EPOCH) diff --git a/src/velocity_estimator.rs b/src/velocity_estimator.rs index 207b58a..bdbe2d7 100644 --- a/src/velocity_estimator.rs +++ b/src/velocity_estimator.rs @@ -9,9 +9,7 @@ //! //! The pipeline, per [`update`](VelocityEstimator::update): //! -//! 1. A raw RPM from the tick/time difference (at the motor's *internal* shaft), -//! with `dt` snapped to the motor's 5 ms sampling grid to reject timestamp -//! jitter. +//! 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 @@ -92,15 +90,12 @@ impl VelocityEstimator { return self.last_output; } - // 1. dt from the motor's own clock, snapped to the motor's 5 ms sampling - // grid. The reported timestamps jitter by a millisecond or two, and - // that jitter would otherwise turn a constant speed into velocity - // quantization noise; rounding to the grid removes it permanently. - // Reads closer together than 5 ms round to 0 -> no new sample yet. - let dt = 5.0 * (timestamp_ms.wrapping_sub(self.previous_timestamp_ms) as f64 / 5.0).round(); - if dt == 0.0 { + // 1. dt from the motor's own 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; @@ -159,16 +154,6 @@ mod tests { assert_eq!(a, b); } - #[test] - fn sub_grid_reads_snap_to_no_new_sample() { - let mut estimator = VelocityEstimator::new(BLUE_RPM); - estimator.update(0, 0); - let a = estimator.update(30, 10); - // Only 2 ms later: rounds to a 0 ms dt, so nothing changes. - let b = estimator.update(45, 12); - assert_eq!(a, b); - } - #[test] fn position_reset_spike_is_ignored() { let mut estimator = VelocityEstimator::new(BLUE_RPM); From 8d9f2777674df7242a61c453144ef4bcb5214916 Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Mon, 7 Sep 2026 23:53:41 -0700 Subject: [PATCH 04/18] Fix reset-recovery, poll rate, and task lifetime in velocity estimation - velocity_estimator: advance the tick/timestamp baseline before the position-reset guard, not after. Previously a real reset_position() left every later sample differenced against the stale pre-reset baseline, so raw_rpm stayed above the threshold forever and the estimator never output again. Matches sylib, which assigns its baseline before the >5000 check. Extends position_reset_spike_is_ignored to assert recovery. - motor_velocity: poll at UPDATE_INTERVAL / 2 (~5ms) instead of UPDATE_INTERVAL. Polling at exactly the ~10ms publish rate would drift past a packet and skip it; oversampling avoids that, and the estimator's dt == 0 early return discards the redundant reads. - motor_velocity: hold the tracking Task in a field instead of detach()ing it, so the task stops when the tracker is dropped. MotorGroupVelocity now owns its MotorVelocityTracker, so each side's task lives exactly as long as the drivetrain holds the source. Co-Authored-By: Claude Opus 4.8 --- src/motor_velocity.rs | 20 +++++++++++++++----- src/velocity_differential.rs | 29 ++++++++++++++--------------- src/velocity_estimator.rs | 28 +++++++++++++++++++++++----- 3 files changed, 52 insertions(+), 25 deletions(-) diff --git a/src/motor_velocity.rs b/src/motor_velocity.rs index 71d2fcd..70a90f4 100644 --- a/src/motor_velocity.rs +++ b/src/motor_velocity.rs @@ -16,6 +16,7 @@ use vexide::{ math::Direction, prelude::sleep, smart::{motor::Motor, SmartDevice}, + task::Task, }; use crate::{ @@ -31,6 +32,9 @@ const DEFAULT_GEARSET_RPM: f64 = 600.0; /// latest per-motor output-shaft RPM through a shared cell. 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 { @@ -54,10 +58,14 @@ impl MotorVelocityTracker { let task_motors = motors.clone(); let task_velocities = velocities.clone(); - vexide::task::spawn(async move { + let task = vexide::task::spawn(async move { let mut estimators = estimators; loop { - sleep(Motor::UPDATE_INTERVAL).await; + // 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. @@ -76,10 +84,12 @@ impl MotorVelocityTracker { results[index] = rpm; } } - }) - .detach(); + }); - Self { velocities } + Self { + velocities, + _task: task, + } } /// A shared handle to the latest per-motor output-shaft RPM, updated in diff --git a/src/velocity_differential.rs b/src/velocity_differential.rs index 1f32a90..abee79b 100644 --- a/src/velocity_differential.rs +++ b/src/velocity_differential.rs @@ -45,11 +45,12 @@ pub trait WheelVelocity { fn velocity(&mut self) -> f64; } -/// A [`WheelVelocity`] source backed by a [`MotorVelocityTracker`]'s shared -/// per-motor RPM cell, averaging the group's filtered output-shaft velocities. +/// A [`WheelVelocity`] source backed by a [`MotorVelocityTracker`], averaging the +/// group's filtered output-shaft velocities. pub struct MotorGroupVelocity { - /// Latest per-motor output-shaft RPM, published by the background tracker. - velocities: 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; the tracker already reports gearset-reduced RPM). `1.0` for direct /// drive. @@ -57,9 +58,9 @@ pub struct MotorGroupVelocity { } impl MotorGroupVelocity { - pub fn new(velocities: Rc>>, gear_ratio: f64) -> Self { + pub fn new(tracker: MotorVelocityTracker, gear_ratio: f64) -> Self { Self { - velocities, + tracker, gear_ratio, } } @@ -67,7 +68,8 @@ impl MotorGroupVelocity { impl WheelVelocity for MotorGroupVelocity { fn velocity(&mut self) -> f64 { - let velocities = self.velocities.borrow(); + let velocities = self.tracker.velocities(); + let velocities = velocities.borrow(); if velocities.is_empty() { return 0.0; } @@ -125,15 +127,12 @@ impl VelocityDifferential { let left: Rc>> = Rc::new(RefCell::new(left)); let right: Rc>> = Rc::new(RefCell::new(right)); - // Each side gets its own background estimator; the trackers can be - // dropped here because their detached tasks keep the shared velocity - // cells alive alongside the sources that read them. + // 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()).velocities(), gear_ratio); - let right_source = MotorGroupVelocity::new( - MotorVelocityTracker::new(right.clone()).velocities(), - gear_ratio, - ); + MotorGroupVelocity::new(MotorVelocityTracker::new(left.clone()), gear_ratio); + let right_source = + MotorGroupVelocity::new(MotorVelocityTracker::new(right.clone()), gear_ratio); Self { left, diff --git a/src/velocity_estimator.rs b/src/velocity_estimator.rs index bdbe2d7..4251b6e 100644 --- a/src/velocity_estimator.rs +++ b/src/velocity_estimator.rs @@ -99,11 +99,20 @@ impl VelocityEstimator { // 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 without disturbing - // the filters or the baseline. + // 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; } @@ -124,8 +133,6 @@ impl VelocityEstimator { let output = self.ema.filter(smoothed, gain) * self.gearset_rpm / INTERNAL_FREE_SPEED_RPM; - self.previous_ticks = ticks; - self.previous_timestamp_ms = timestamp_ms; self.last_output = output; output } @@ -160,9 +167,20 @@ mod tests { 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. + // 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] From 0dfdff8bb55bbf1a58b53cdcbf299e62ab3ba371 Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Mon, 7 Sep 2026 23:56:39 -0700 Subject: [PATCH 05/18] Fix Median doc, add direction probe, document startup and sampling - filters: Median::filter returns the upper of the two middles for an even window (sorted[len/2]); fix the comment, which claimed the lower. - sensor: add probe_direction(), a one-shot hardware diagnostic that drives a Reverse-configured motor at +3V and prints the sign of the raw_position() change plus the value MOTOR_RAW_POSITION_RESPECTS_DIRECTION should hold. The constant stays an unverified guess until measured. - velocity_estimator: document that partial-window filtering during the first ~20 samples is intentional (matches sylib), avoiding ~200ms of false zero velocity a feedback loop would integrate as error. - motor_velocity: document that fixed-rate background sampling means the shared cell may be up to one poll period stale by design, keeping the sample-count filter time constants tied to the poll rate rather than a variable control-loop period. Co-Authored-By: Claude Opus 4.8 --- src/filters.rs | 2 +- src/motor_velocity.rs | 9 +++++++++ src/sensor.rs | 40 +++++++++++++++++++++++++++++++++++++++ src/velocity_estimator.rs | 8 ++++++++ 4 files changed, 58 insertions(+), 1 deletion(-) diff --git a/src/filters.rs b/src/filters.rs index 13352a4..8d5b00d 100644 --- a/src/filters.rs +++ b/src/filters.rs @@ -50,7 +50,7 @@ impl Median { } } - /// Pushes `input` and returns the middle of the sorted window (the lower of + /// 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 { diff --git a/src/motor_velocity.rs b/src/motor_velocity.rs index 70a90f4..8211c61 100644 --- a/src/motor_velocity.rs +++ b/src/motor_velocity.rs @@ -9,6 +9,15 @@ //! 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, rc::Rc}; diff --git a/src/sensor.rs b/src/sensor.rs index 6f400ec..534d573 100644 --- a/src/sensor.rs +++ b/src/sensor.rs @@ -5,7 +5,11 @@ //! 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, }; @@ -44,3 +48,39 @@ impl TimestampedPosition for Motor { Ok((ticks, timestamp)) } } + +/// One-shot hardware diagnostic for [`MOTOR_RAW_POSITION_RESPECTS_DIRECTION`]. +/// +/// Configures `motor` as [`Direction::Reverse`], drives it at a low positive +/// voltage for ~500 ms, and prints the sign of the resulting change in +/// `raw_position()` along with the value the constant should hold. Leaves the +/// motor stopped. Run once against a free-spinning motor and set the constant to +/// match; nothing in the normal code path calls this. +// One-shot diagnostic, wired up by hand when characterizing hardware. +#[allow(dead_code)] +pub async fn probe_direction(motor: &mut Motor) { + let _ = motor.set_direction(Direction::Reverse); + + let start = motor.raw_position().unwrap_or(0); + let _ = motor.set_voltage(3.0); + sleep(Duration::from_millis(500)).await; + let end = motor.raw_position().unwrap_or(start); + let _ = motor.set_voltage(0.0); + + // A Reverse-configured motor driven at *positive* voltage spins physically + // backward. If raw_position() honors the direction flag its reported ticks go + // negative; if it reports the bare encoder they go positive. + let delta = end - start; + if delta == 0 { + println!( + "probe_direction: raw_position() did not change over 500 ms at +3 V \ + (motor stalled or disconnected?) — inconclusive." + ); + return; + } + let respects_direction = delta < 0; + println!( + "probe_direction: raw_position() delta = {delta} over 500 ms at +3 V (Reverse). \ + Set MOTOR_RAW_POSITION_RESPECTS_DIRECTION = {respects_direction}." + ); +} diff --git a/src/velocity_estimator.rs b/src/velocity_estimator.rs index 4251b6e..5a17081 100644 --- a/src/velocity_estimator.rs +++ b/src/velocity_estimator.rs @@ -19,6 +19,14 @@ //! All time in this file is in **milliseconds**. The gain constants in step 8 //! 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)). From 942c3a96de475ae8d1838a78219ca5d322b3b28c Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Tue, 8 Sep 2026 18:17:27 -0700 Subject: [PATCH 06/18] Fix probe_direction sign logic, measure TPR, document TICKS_PER_INTERNAL_REV MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - sensor: rewrite probe_direction. The old "reverse + positive voltage -> negative ticks if honored" reasoning was inverted (the flag inverts command and sensor together). Instead compare raw_position() against position(), which definitively applies the flag: they agree iff raw honors it. Requires Direction::Reverse to discriminate, and always restores the original direction (and stops the motor) on every path, including errors. Now returns Result. - sensor: the same run also reports ticks per internal revolution (raw ticks per output rev / gearset reduction); expect ~50. - velocity_estimator: replace the TICKS_PER_INTERNAL_REV "verify on hardware" TODO with the evidence — VEX's per-output-rev figures (1800/900/300, i.e. vexide's Gearset::*_TICKS_PER_REVOLUTION) each over their reduction give 50. vexide's raw_position doc claims 4096, which contradicts its own gearset constants and is wrong. - motor_velocity/velocity_differential: add MotorVelocityTracker::with_velocities to read the shared cell without cloning an Rc every control iteration; MotorGroupVelocity::velocity uses it. Removed the now-unused velocities() accessor. Co-Authored-By: Claude Opus 4.8 --- src/motor_velocity.rs | 8 ++-- src/sensor.rs | 79 ++++++++++++++++++++++++++---------- src/velocity_differential.rs | 17 ++++---- src/velocity_estimator.rs | 9 +++- 4 files changed, 77 insertions(+), 36 deletions(-) diff --git a/src/motor_velocity.rs b/src/motor_velocity.rs index 8211c61..7349337 100644 --- a/src/motor_velocity.rs +++ b/src/motor_velocity.rs @@ -101,9 +101,9 @@ impl MotorVelocityTracker { } } - /// A shared handle to the latest per-motor output-shaft RPM, updated in - /// place by the background task. - pub fn velocities(&self) -> Rc>> { - self.velocities.clone() + /// Runs `f` over the latest per-motor output-shaft RPM without cloning the + /// shared `Rc` — for hot-path readers called every control iteration. + pub fn with_velocities(&self, f: impl FnOnce(&[f64]) -> R) -> R { + f(&self.velocities.borrow()) } } diff --git a/src/sensor.rs b/src/sensor.rs index 534d573..b582286 100644 --- a/src/sensor.rs +++ b/src/sensor.rs @@ -49,38 +49,73 @@ impl TimestampedPosition for Motor { } } -/// One-shot hardware diagnostic for [`MOTOR_RAW_POSITION_RESPECTS_DIRECTION`]. +/// One-shot hardware diagnostic for [`MOTOR_RAW_POSITION_RESPECTS_DIRECTION`] and +/// `TICKS_PER_INTERNAL_REV`. /// -/// Configures `motor` as [`Direction::Reverse`], drives it at a low positive -/// voltage for ~500 ms, and prints the sign of the resulting change in -/// `raw_position()` along with the value the constant should hold. Leaves the -/// motor stopped. Run once against a free-spinning motor and set the constant to -/// match; nothing in the normal code path calls this. +/// 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. // One-shot diagnostic, wired up by hand when characterizing hardware. #[allow(dead_code)] -pub async fn probe_direction(motor: &mut Motor) { - let _ = motor.set_direction(Direction::Reverse); +pub async fn probe_direction(motor: &mut Motor) -> Result<(), PortError> { + let original = motor.direction()?; + motor.set_direction(Direction::Reverse)?; - let start = motor.raw_position().unwrap_or(0); - let _ = motor.set_voltage(3.0); - sleep(Duration::from_millis(500)).await; - let end = motor.raw_position().unwrap_or(start); - let _ = motor.set_voltage(0.0); + // 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?; - // A Reverse-configured motor driven at *positive* voltage spins physically - // backward. If raw_position() honors the direction flag its reported ticks go - // negative; if it reports the bare encoder they go positive. - let delta = end - start; - if delta == 0 { + // Keep the stalled/disconnected guard: no raw motion means nothing to compare. + if raw_delta == 0 { println!( "probe_direction: raw_position() did not change over 500 ms at +3 V \ (motor stalled or disconnected?) — inconclusive." ); - return; + return Ok(()); } - let respects_direction = delta < 0; + + // `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_position() delta = {delta} over 500 ms at +3 V (Reverse). \ - Set MOTOR_RAW_POSITION_RESPECTS_DIRECTION = {respects_direction}." + "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. +#[allow(dead_code)] +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/velocity_differential.rs b/src/velocity_differential.rs index abee79b..184e2e1 100644 --- a/src/velocity_differential.rs +++ b/src/velocity_differential.rs @@ -68,14 +68,15 @@ impl MotorGroupVelocity { impl WheelVelocity for MotorGroupVelocity { fn velocity(&mut self) -> f64 { - let velocities = self.tracker.velocities(); - let velocities = velocities.borrow(); - if velocities.is_empty() { - return 0.0; - } - let mean_rpm = velocities.iter().sum::() / velocities.len() as f64; - // motor output RPM -> wheel RPM -> wheel rad/s - mean_rpm * self.gear_ratio * (2.0 * PI / 60.0) + let gear_ratio = self.gear_ratio; + self.tracker.with_velocities(|velocities| { + if velocities.is_empty() { + return 0.0; + } + let mean_rpm = velocities.iter().sum::() / velocities.len() as f64; + // motor output RPM -> wheel RPM -> wheel rad/s + mean_rpm * gear_ratio * (2.0 * PI / 60.0) + }) } } diff --git a/src/velocity_estimator.rs b/src/velocity_estimator.rs index 5a17081..6da387c 100644 --- a/src/velocity_estimator.rs +++ b/src/velocity_estimator.rs @@ -35,8 +35,13 @@ use crate::filters::{Derivative, Ema, MaxAbs, Median, Sma}; /// Raw encoder ticks per revolution of the motor's *internal* (pre-gearset) /// shaft. -// TODO: verify on hardware by spinning one full output revolution and diffing -// raw_position(). +// +// 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 From 79efef7ef9f56680870fbcc2a0b6c9f0ddd94ef9 Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Wed, 9 Sep 2026 18:16:44 -0700 Subject: [PATCH 07/18] Correct and tighten comments across the velocity-estimation changes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Audited every comment in the PR's files for accuracy and concision: - sensor: MOTOR_RAW_POSITION_RESPECTS_DIRECTION doc carried the inverted sign description that the probe_direction fix corrected — "respects direction" means raw_position() matches position()'s sign, not "reads negative when driven forward". Point the TODO at probe_direction and drop a redundant diagnostic comment. - motor_velocity: module doc claimed the sysid collector reads this tracker's cell; it doesn't (sysid drives VelocityEstimator directly). - velocity_differential: the feedback-read comment implied a motor-borrow ordering constraint that no longer exists now that the source reads a separate cell. - sysid: module doc's transient block and paste rules predated the raw-omega z_1 list; document it. gear_ratio note now points at the estimator rather than Motor::velocity. - velocity_estimator: module doc referenced a "step 8" absent from its own summary list; state the steady-state gain floor and settling lag concretely. Co-Authored-By: Claude Opus 4.8 --- src/motor_velocity.rs | 8 ++++++-- src/sensor.rs | 15 +++++++-------- src/sysid.rs | 15 ++++++++------- src/velocity_differential.rs | 4 ++-- src/velocity_estimator.rs | 11 +++++++---- 5 files changed, 30 insertions(+), 23 deletions(-) diff --git a/src/motor_velocity.rs b/src/motor_velocity.rs index 7349337..1375d7d 100644 --- a/src/motor_velocity.rs +++ b/src/motor_velocity.rs @@ -3,8 +3,8 @@ //! [`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 consumers (the drivetrain's velocity -//! feedback and the sysid collector) read without touching the motors directly. +//! 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 @@ -81,6 +81,10 @@ impl MotorVelocityTracker { let mut motors = task_motors.borrow_mut(); let mut results = task_velocities.borrow_mut(); for (index, motor) in motors.as_mut().iter().enumerate() { + // TODO: on error a motor keeps its last `results[index]`, which + // `MotorGroupVelocity::velocity()` still averages in — a + // disconnected motor biases the side mean toward a stale/zero + // value. Consider tracking per-motor validity. let Ok((ticks, timestamp)) = motor.timestamped_position() else { continue; }; diff --git a/src/sensor.rs b/src/sensor.rs index b582286..1a0c3b1 100644 --- a/src/sensor.rs +++ b/src/sensor.rs @@ -14,13 +14,13 @@ use vexide::{ time::LowResolutionTime, }; -/// Whether `Motor::raw_position()` already accounts for the motor's configured -/// [`Direction`](vexide::smart::motor::Direction) (so a reversed motor reads -/// negative ticks when driven "forward"). +/// Whether `Motor::raw_position()` applies the motor's configured +/// [`Direction`](vexide::smart::motor::Direction) — i.e. reports the same sign as +/// `Motor::position()`. /// -/// If this is `false`, callers must negate the estimator's output for motors -/// configured [`Direction::Reverse`](vexide::smart::motor::Direction::Reverse). -// TODO: verify on hardware. +/// 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 device's own @@ -61,7 +61,6 @@ impl TimestampedPosition for Motor { /// 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. -// One-shot diagnostic, wired up by hand when characterizing hardware. #[allow(dead_code)] pub async fn probe_direction(motor: &mut Motor) -> Result<(), PortError> { let original = motor.direction()?; @@ -73,7 +72,7 @@ pub async fn probe_direction(motor: &mut Motor) -> Result<(), PortError> { motor.set_direction(original)?; let (raw_delta, pos_delta) = measured?; - // Keep the stalled/disconnected guard: no raw motion means nothing to compare. + // No raw motion means the motor stalled or is disconnected: nothing to compare. if raw_delta == 0 { println!( "probe_direction: raw_position() did not change over 500 ms at +3 V \ diff --git a/src/sysid.rs b/src/sysid.rs index dbb2ac3..00f8657 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. //! @@ -93,7 +94,7 @@ pub struct SysIdConfig { /// 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, } diff --git a/src/velocity_differential.rs b/src/velocity_differential.rs index 184e2e1..ba02c36 100644 --- a/src/velocity_differential.rs +++ b/src/velocity_differential.rs @@ -252,8 +252,8 @@ where (0.0, 0.0) }; - // Read the velocity feedback from the trackers' shared cells before - // borrowing the motors for writing. + // 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 index 6da387c..73a1efb 100644 --- a/src/velocity_estimator.rs +++ b/src/velocity_estimator.rs @@ -16,8 +16,8 @@ //! 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 gain constants in step 8 -//! are calibrated to that scale, so do not convert to seconds anywhere here. +//! 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 //! @@ -138,8 +138,11 @@ impl VelocityEstimator { let accel = self.derivative.filter(median, dt); // 7. ...and its recent peak magnitude. let peak = self.max_abs_20.filter(accel); - // 8. Adaptive gain: near 0 when steady, rising toward 0.75 during - // acceleration transients so the estimate keeps up. + // 8. Adaptive gain: floors at ~0.01 (peak == 0 gives 0.75*(1 - 1/1.013)) + // when steady, rising toward 0.75 during acceleration transients so the + // estimate keeps up. The low floor means slow steady-state settling — + // ~100 samples (~1 s at the motor's ~10 ms data interval); deliberate + // heavy smoothing, so account for it in feedback tuning. let gain = 0.75 * (1.0 - 1.0 / ((peak * peak / 50.0) + 1.013)); // 9. EMA the *smoothed* value with the adaptive gain, then convert // internal-shaft RPM to output-shaft RPM. From 4d50b094058ccf491e310835b75e4dfdde38302f Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Wed, 9 Sep 2026 19:06:41 -0700 Subject: [PATCH 08/18] Exclude failed motors from feedback, guard probe divisor, sync sysid rate - motor_velocity: publish Vec>. A failed read now writes None at that index instead of leaving a stale value, so a motor unplugged mid-match is dropped from the mean rather than feeding the drivetrain fiction. with_velocities exposes &[Option]. - velocity_differential: MotorGroupVelocity::velocity() averages only the Some entries, still returning 0.0 when none are live. - sensor: probe_direction's inconclusive guard also covers pos_delta == 0.0, which would otherwise divide to inf/NaN in the ticks-per-rev printout. - sysid: default sample_interval is 5ms to match MotorVelocityTracker's Motor::UPDATE_INTERVAL / 2 poll rate; the estimator's sample-count filter windows then have identical time constants during identification and in the control loop, so the fitted Kv/Ka describe the estimator actually run. Co-Authored-By: Claude Opus 4.8 --- src/motor_velocity.rs | 24 +++++++++++++----------- src/sensor.rs | 5 +++-- src/sysid.rs | 8 ++++++-- src/velocity_differential.rs | 13 ++++++++++--- 4 files changed, 32 insertions(+), 18 deletions(-) diff --git a/src/motor_velocity.rs b/src/motor_velocity.rs index 1375d7d..1ea8308 100644 --- a/src/motor_velocity.rs +++ b/src/motor_velocity.rs @@ -38,9 +38,11 @@ use crate::{ const DEFAULT_GEARSET_RPM: f64 = 600.0; /// Runs a [`VelocityEstimator`] per motor on a background task, exposing the -/// latest per-motor output-shaft RPM through a shared cell. +/// 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>>, + 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<()>, @@ -63,7 +65,7 @@ impl MotorVelocityTracker { } } - let velocities = Rc::new(RefCell::new(vec![0.0; estimators.len()])); + let velocities = Rc::new(RefCell::new(vec![None; estimators.len()])); let task_motors = motors.clone(); let task_velocities = velocities.clone(); @@ -81,11 +83,10 @@ impl MotorVelocityTracker { let mut motors = task_motors.borrow_mut(); let mut results = task_velocities.borrow_mut(); for (index, motor) in motors.as_mut().iter().enumerate() { - // TODO: on error a motor keeps its last `results[index]`, which - // `MotorGroupVelocity::velocity()` still averages in — a - // disconnected motor biases the side mean toward a stale/zero - // value. Consider tracking per-motor validity. + // 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); @@ -94,7 +95,7 @@ impl MotorVelocityTracker { { rpm = -rpm; } - results[index] = rpm; + results[index] = Some(rpm); } } }); @@ -105,9 +106,10 @@ impl MotorVelocityTracker { } } - /// Runs `f` over the latest per-motor output-shaft RPM without cloning the - /// shared `Rc` — for hot-path readers called every control iteration. - pub fn with_velocities(&self, f: impl FnOnce(&[f64]) -> R) -> R { + /// 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 index 1a0c3b1..c884bbb 100644 --- a/src/sensor.rs +++ b/src/sensor.rs @@ -72,8 +72,9 @@ pub async fn probe_direction(motor: &mut Motor) -> Result<(), PortError> { motor.set_direction(original)?; let (raw_delta, pos_delta) = measured?; - // No raw motion means the motor stalled or is disconnected: nothing to compare. - if raw_delta == 0 { + // No motion on either reading means the motor stalled or is disconnected: + // nothing to compare, and `pos_delta` would divide to inf/NaN below. + if raw_delta == 0 || pos_delta == 0.0 { println!( "probe_direction: raw_position() did not change over 500 ms at +3 V \ (motor stalled or disconnected?) — inconclusive." diff --git a/src/sysid.rs b/src/sysid.rs index 00f8657..0411190 100644 --- a/src/sysid.rs +++ b/src/sysid.rs @@ -89,7 +89,11 @@ 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. + /// Sampling period. Must match + /// [`MotorVelocityTracker`](crate::motor_velocity::MotorVelocityTracker)'s poll + /// rate (`Motor::UPDATE_INTERVAL / 2` = 5 ms) so the estimator's sample-count + /// filter windows have identical time constants during identification and in + /// the control loop. Changing it invalidates the fitted `Kv`/`Ka`. pub sample_interval: Duration, /// Wheel revolutions per motor output-shaft revolution — the *same* /// `gear_ratio` passed to `VelocityDifferential`, so the fitted constants @@ -104,7 +108,7 @@ 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, } } diff --git a/src/velocity_differential.rs b/src/velocity_differential.rs index ba02c36..8b49d28 100644 --- a/src/velocity_differential.rs +++ b/src/velocity_differential.rs @@ -70,12 +70,19 @@ impl WheelVelocity for MotorGroupVelocity { fn velocity(&mut self) -> f64 { let gear_ratio = self.gear_ratio; self.tracker.with_velocities(|velocities| { - if velocities.is_empty() { + // Average only the live motors; a failed read contributes `None` and + // is excluded rather than dragging the mean toward a stale value. + let mut sum_rpm = 0.0; + let mut count = 0.0; + for &rpm in velocities.iter().flatten() { + sum_rpm += rpm; + count += 1.0; + } + if count == 0.0 { return 0.0; } - let mean_rpm = velocities.iter().sum::() / velocities.len() as f64; // motor output RPM -> wheel RPM -> wheel rad/s - mean_rpm * gear_ratio * (2.0 * PI / 60.0) + (sum_rpm / count) * gear_ratio * (2.0 * PI / 60.0) }) } } From 12be970ba93e40d5e413af9c563e41d6d42bed0f Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Thu, 10 Sep 2026 19:47:20 -0700 Subject: [PATCH 09/18] Exclude failed motors from sysid mean; fix probe inconclusive message MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - sysid: mirror the motor_velocity fix. update_estimators writes None on a failed read (buffer is now Vec>), and mean_omega averages only the Some entries, returning 0.0 when none are live — so a flaky motor no longer biases the fitted Kv/Ka with a stale value. - sensor: probe_direction's inconclusive message no longer names only raw_position(); it now reports both measured deltas so the pos_delta == 0 branch is legible. Co-Authored-By: Claude Opus 4.8 --- src/sensor.rs | 6 +++--- src/sysid.rs | 28 ++++++++++++++++++---------- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/sensor.rs b/src/sensor.rs index c884bbb..2928d24 100644 --- a/src/sensor.rs +++ b/src/sensor.rs @@ -73,11 +73,11 @@ pub async fn probe_direction(motor: &mut Motor) -> Result<(), PortError> { let (raw_delta, pos_delta) = measured?; // No motion on either reading means the motor stalled or is disconnected: - // nothing to compare, and `pos_delta` would divide to inf/NaN below. + // nothing to compare, and `pos_delta == 0` would divide to inf/NaN below. if raw_delta == 0 || pos_delta == 0.0 { println!( - "probe_direction: raw_position() did not change over 500 ms at +3 V \ - (motor stalled or disconnected?) — inconclusive." + "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(()); } diff --git a/src/sysid.rs b/src/sysid.rs index 0411190..f76c575 100644 --- a/src/sysid.rs +++ b/src/sysid.rs @@ -174,7 +174,7 @@ async fn run_step( ) -> Vec<(f64, f64, f64)> { let mut samples = Vec::new(); let mut estimators = build_estimators(left, right); - let mut velocities = vec![0.0; estimators.len()]; + let mut velocities = vec![None; estimators.len()]; let start = Instant::now(); while start.elapsed() < config.hold { @@ -205,16 +205,18 @@ fn build_estimators(left: &[Motor], right: &[Motor]) -> Vec { } /// Feeds one timestamped sample into every estimator, writing the resulting -/// per-motor output-shaft RPM into `velocities` (left then right). Motors that -/// error out keep their previous value. +/// per-motor output-shaft RPM into `velocities` (left then right). A motor whose +/// read fails gets `None`, so `mean_omega` excludes it rather than reusing a +/// stale value. fn update_estimators( left: &[Motor], right: &[Motor], estimators: &mut [VelocityEstimator], - velocities: &mut [f64], + velocities: &mut [Option], ) { for (index, motor) in left.iter().chain(right.iter()).enumerate() { let Ok((ticks, timestamp)) = motor.timestamped_position() else { + velocities[index] = None; continue; }; let mut rpm = estimators[index].update(ticks, timestamp); @@ -223,7 +225,7 @@ fn update_estimators( { rpm = -rpm; } - velocities[index] = rpm; + velocities[index] = Some(rpm); } } @@ -289,13 +291,19 @@ fn set_all(left: &mut [Motor], right: &mut [Motor], volts: f64) { /// Mean wheel angular velocity (rad/s) from the estimator pipeline's per-motor /// output-shaft RPM in `velocities`, converting to wheel rad/s exactly as -/// `MotorGroupVelocity` does. -fn mean_omega(velocities: &[f64], gear_ratio: f64) -> f64 { - if velocities.is_empty() { +/// `MotorGroupVelocity` does. Motors whose read failed (`None`) are excluded; +/// returns `0.0` when none are live. +fn mean_omega(velocities: &[Option], gear_ratio: f64) -> f64 { + let mut sum_rpm = 0.0; + let mut count = 0.0; + for &rpm in velocities.iter().flatten() { + sum_rpm += rpm; + count += 1.0; + } + if count == 0.0 { return 0.0; } - let mean_rpm = velocities.iter().sum::() / velocities.len() as f64; - mean_rpm * gear_ratio * (2.0 * PI / 60.0) + (sum_rpm / count) * gear_ratio * (2.0 * PI / 60.0) } /// Mean wheel angular velocity (rad/s) from the motors' own *unfiltered* From 2cd1028271427d333ae772c99c3e07ad20081163 Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Sat, 12 Sep 2026 00:16:10 -0700 Subject: [PATCH 10/18] Make sysid reuse MotorVelocityTracker instead of a duplicate loop Wrap each side's motors in an Rc>> in main and hand clones to both sysid and the drivetrain. sysid::collect now builds one MotorVelocityTracker per side for the whole run, so steps start with warm filter windows instead of empty ones, and the RPM->rad/s conversion and live-motor averaging are shared free functions used by both MotorGroupVelocity and sysid. sample_interval now controls logging density only. Co-Authored-By: Claude Opus 4.8 --- src/main.rs | 20 +++-- src/sysid.rs | 144 ++++++++++++++--------------------- src/velocity_differential.rs | 49 ++++++------ 3 files changed, 96 insertions(+), 117 deletions(-) diff --git a/src/main.rs b/src/main.rs index 0c2f20b..74e314d 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::*; @@ -111,23 +111,29 @@ impl Compete for Robot { 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 = [ + let 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), ]; - let mut right_motors = [ + let 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), ]; + // 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(), &SysIdConfig { // TODO: set this to the drivetrain's real wheel-per-motor gear // ratio (the same value passed to `VelocityDifferential::new` @@ -146,8 +152,8 @@ 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, diff --git a/src/sysid.rs b/src/sysid.rs index f76c575..35f75f4 100644 --- a/src/sysid.rs +++ b/src/sysid.rs @@ -57,24 +57,19 @@ //! enough that the robot fully coasts back to rest between steps. use std::{ + cell::RefCell, f64::consts::PI, + rc::Rc, time::{Duration, Instant}, }; -use vexide::{ - math::Direction, - prelude::{sleep, Motor}, -}; +use vexide::prelude::{sleep, Motor}; use crate::{ - sensor::{TimestampedPosition, MOTOR_RAW_POSITION_RESPECTS_DIRECTION}, - velocity_estimator::VelocityEstimator, + motor_velocity::MotorVelocityTracker, + velocity_differential::{live_rpm_sum, wheel_omega_from_rpm}, }; -/// Fallback output-shaft free speed (blue cartridge) if a motor's gearset can't -/// be read while building its estimator. -const DEFAULT_GEARSET_RPM: f64 = 600.0; - /// Fraction of each step's samples (from the end) averaged for the settled /// speed that feeds the steady-state fit. const SETTLE_TAIL: f64 = 0.30; @@ -89,11 +84,12 @@ pub struct SysIdConfig { pub hold: Duration, /// How long to coast between steps so the robot returns to rest. pub rest: Duration, - /// Sampling period. Must match - /// [`MotorVelocityTracker`](crate::motor_velocity::MotorVelocityTracker)'s poll - /// rate (`Motor::UPDATE_INTERVAL / 2` = 5 ms) so the estimator's sample-count - /// filter windows have identical time constants during identification and in - /// the control loop. Changing it invalidates the fitted `Kv`/`Ka`. + /// 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 @@ -116,8 +112,9 @@ impl Default for SysIdConfig { /// One staircase step: its label, the signed voltage held, and the buffered /// `(t, estimated_omega, raw_omega)` samples of the rise. `estimated_omega` comes -/// from the [`VelocityEstimator`] pipeline; `raw_omega` is the motor's own -/// unfiltered velocity, kept alongside so the two can be compared in Desmos. +/// 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, @@ -131,7 +128,18 @@ 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>>, + 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()); + let right_tracker = MotorVelocityTracker::new(right.clone()); + let mut steps = Vec::new(); // Interleave direction per level — run each level forward then immediately @@ -141,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, @@ -149,13 +157,13 @@ 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); } @@ -164,24 +172,41 @@ pub async fn collect(left: &mut [Motor], right: &mut [Motor], config: &SysIdConf /// `(t, estimated_omega, raw_omega)` sample every `config.sample_interval`. `t` /// is measured from the start of this step. /// -/// A fresh [`VelocityEstimator`] per motor is built for each step so the filter -/// state doesn't carry across the coast between steps. +/// 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, f64)> { let mut samples = Vec::new(); - let mut estimators = build_estimators(left, right); - let mut velocities = vec![None; estimators.len()]; let start = Instant::now(); while start.elapsed() < config.hold { - set_all(left, right, volts); - update_estimators(left, right, &mut estimators, &mut velocities); - let estimated = mean_omega(&velocities, config.gear_ratio); - let raw = mean_raw_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, estimated, raw)); sleep(config.sample_interval).await; @@ -189,46 +214,6 @@ async fn run_step( samples } -/// One [`VelocityEstimator`] per drive motor (left then right), each seeded with -/// its motor's gearset free speed. -fn build_estimators(left: &[Motor], right: &[Motor]) -> Vec { - left.iter() - .chain(right.iter()) - .map(|motor| { - let gearset_rpm = motor - .gearset() - .map(|gearset| gearset.max_rpm()) - .unwrap_or(DEFAULT_GEARSET_RPM); - VelocityEstimator::new(gearset_rpm) - }) - .collect() -} - -/// Feeds one timestamped sample into every estimator, writing the resulting -/// per-motor output-shaft RPM into `velocities` (left then right). A motor whose -/// read fails gets `None`, so `mean_omega` excludes it rather than reusing a -/// stale value. -fn update_estimators( - left: &[Motor], - right: &[Motor], - estimators: &mut [VelocityEstimator], - velocities: &mut [Option], -) { - for (index, motor) in left.iter().chain(right.iter()).enumerate() { - let Ok((ticks, timestamp)) = motor.timestamped_position() else { - velocities[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; - } - velocities[index] = Some(rpm); - } -} - /// Prints the collected steps as Desmos list literals: one steady-state block /// for `Ks`/`Kv`, then one transient block per step for `Ka`. Each list is /// alone on its own line with a fixed-decimal, scientific-notation-free format @@ -289,23 +274,6 @@ fn set_all(left: &mut [Motor], right: &mut [Motor], volts: f64) { } } -/// Mean wheel angular velocity (rad/s) from the estimator pipeline's per-motor -/// output-shaft RPM in `velocities`, converting to wheel rad/s exactly as -/// `MotorGroupVelocity` does. Motors whose read failed (`None`) are excluded; -/// returns `0.0` when none are live. -fn mean_omega(velocities: &[Option], gear_ratio: f64) -> f64 { - let mut sum_rpm = 0.0; - let mut count = 0.0; - for &rpm in velocities.iter().flatten() { - sum_rpm += rpm; - count += 1.0; - } - if count == 0.0 { - return 0.0; - } - (sum_rpm / count) * gear_ratio * (2.0 * PI / 60.0) -} - /// 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 diff --git a/src/velocity_differential.rs b/src/velocity_differential.rs index 8b49d28..dfcab0b 100644 --- a/src/velocity_differential.rs +++ b/src/velocity_differential.rs @@ -45,6 +45,26 @@ pub trait WheelVelocity { fn velocity(&mut self) -> f64; } +/// 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 [`MotorGroupVelocity`] 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) +} + /// A [`WheelVelocity`] source backed by a [`MotorVelocityTracker`], averaging the /// group's filtered output-shaft velocities. pub struct MotorGroupVelocity { @@ -70,19 +90,11 @@ impl WheelVelocity for MotorGroupVelocity { fn velocity(&mut self) -> f64 { let gear_ratio = self.gear_ratio; self.tracker.with_velocities(|velocities| { - // Average only the live motors; a failed read contributes `None` and - // is excluded rather than dragging the mean toward a stale value. - let mut sum_rpm = 0.0; - let mut count = 0.0; - for &rpm in velocities.iter().flatten() { - sum_rpm += rpm; - count += 1.0; - } - if count == 0.0 { + let (sum_rpm, count) = live_rpm_sum(velocities); + if count == 0 { return 0.0; } - // motor output RPM -> wheel RPM -> wheel rad/s - (sum_rpm / count) * gear_ratio * (2.0 * PI / 60.0) + wheel_omega_from_rpm(sum_rpm / count as f64, gear_ratio) }) } } @@ -122,19 +134,12 @@ impl VelocityDifferential { /// Reads its velocity feedback from the drive motors' own encoders. /// `gear_ratio` configures the built-in [`MotorGroupVelocity`] sources; 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, 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)); - + ) -> 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 = From 30ea5350c8e6720b3e8d1697f5395a32ebdbbb3a Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Sat, 12 Sep 2026 00:20:33 -0700 Subject: [PATCH 11/18] Move RPM-averaging helpers to motor_velocity; share conversion in sysid live_rpm_sum and wheel_omega_from_rpm move from velocity_differential to motor_velocity, next to the tracker that produces the values they consume; neither has anything to do with the differential model. mean_raw_omega now routes through wheel_omega_from_rpm so the estimated (y_1) and raw (z_1) Desmos series go through the same conversion. Co-Authored-By: Claude Opus 4.8 --- src/motor_velocity.rs | 22 +++++++++++++++++++++- src/sysid.rs | 8 ++------ src/velocity_differential.rs | 23 +---------------------- 3 files changed, 24 insertions(+), 29 deletions(-) diff --git a/src/motor_velocity.rs b/src/motor_velocity.rs index 1ea8308..5ca7efe 100644 --- a/src/motor_velocity.rs +++ b/src/motor_velocity.rs @@ -19,7 +19,7 @@ //! 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, rc::Rc}; +use std::{cell::RefCell, f64::consts::PI, rc::Rc}; use vexide::{ math::Direction, @@ -37,6 +37,26 @@ use crate::{ /// can't be read while building its estimator. const DEFAULT_GEARSET_RPM: f64 = 600.0; +/// 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 diff --git a/src/sysid.rs b/src/sysid.rs index 35f75f4..327fc36 100644 --- a/src/sysid.rs +++ b/src/sysid.rs @@ -58,17 +58,13 @@ use std::{ cell::RefCell, - f64::consts::PI, rc::Rc, time::{Duration, Instant}, }; use vexide::prelude::{sleep, Motor}; -use crate::{ - motor_velocity::MotorVelocityTracker, - velocity_differential::{live_rpm_sum, wheel_omega_from_rpm}, -}; +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. @@ -291,5 +287,5 @@ fn mean_raw_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 dfcab0b..010bdf7 100644 --- a/src/velocity_differential.rs +++ b/src/velocity_differential.rs @@ -25,7 +25,6 @@ use std::{ cell::RefCell, - f64::consts::PI, rc::Rc, time::{Duration, Instant}, }; @@ -37,7 +36,7 @@ use evian::{ }; use vexide::{prelude::Motor, smart::PortError}; -use crate::motor_velocity::MotorVelocityTracker; +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. @@ -45,26 +44,6 @@ pub trait WheelVelocity { fn velocity(&mut self) -> f64; } -/// 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 [`MotorGroupVelocity`] 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) -} - /// A [`WheelVelocity`] source backed by a [`MotorVelocityTracker`], averaging the /// group's filtered output-shaft velocities. pub struct MotorGroupVelocity { From d5706a99044f35ee6a5626c403cab10a81733d07 Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Sat, 12 Sep 2026 00:31:06 -0700 Subject: [PATCH 12/18] Correct comments: timestamp is the Brain's clock, not a motor clock V5 motors transmit no timestamp; Motor::timestamp() and the vexDeviceMotorPositionRawGet out-param both return vexSystemTimeGet() sampled when CPU1 published the packet. Replace the stale TODO promising an out-param swap and reword "the motor's own clock" to the Brain's clock across sensor and velocity_estimator. Co-Authored-By: Claude Opus 4.8 --- src/sensor.rs | 16 ++++++++++------ src/velocity_estimator.rs | 6 +++--- 2 files changed, 13 insertions(+), 9 deletions(-) diff --git a/src/sensor.rs b/src/sensor.rs index 2928d24..1ec3c44 100644 --- a/src/sensor.rs +++ b/src/sensor.rs @@ -1,7 +1,7 @@ //! Position/time sampling for the velocity estimator. //! //! The [`VelocityEstimator`](crate::velocity_estimator::VelocityEstimator) -//! differentiates raw encoder ticks against the device's own clock. This module +//! 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`]. @@ -23,8 +23,8 @@ use vexide::{ // 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 device's own -/// clock reading, both sampled as close together as the API allows. +/// 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, device clock reading in milliseconds). @@ -37,9 +37,13 @@ impl TimestampedPosition for Motor { fn timestamped_position(&self) -> Result<(i32, u32), Self::Error> { let ticks = self.raw_position()?; - // TODO: this is the Brain's packet-processed timestamp, not the motor's own record - // of when it sampled, and the two reads below may describe different samples. Swap - // to the vexDeviceMotorPositionRawGet out-param once vexide exposes it (vexide#386). + // `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) diff --git a/src/velocity_estimator.rs b/src/velocity_estimator.rs index 73a1efb..8c72b9c 100644 --- a/src/velocity_estimator.rs +++ b/src/velocity_estimator.rs @@ -3,7 +3,7 @@ //! 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 motor's own clock and runs the result through a small +//! position against the Brain's clock and runs the result through a small //! filter chain, closely following sylib's approach: //! //! @@ -93,7 +93,7 @@ impl VelocityEstimator { /// output-shaft velocity in RPM. /// /// `ticks` is the raw (pre-gearset) encoder count and `timestamp_ms` is the - /// motor's own clock reading in milliseconds. + /// 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 { @@ -103,7 +103,7 @@ impl VelocityEstimator { return self.last_output; } - // 1. dt from the motor's own clock. No elapsed time -> nothing new to say. + // 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; From 88a69e49644cf152ac6b0a5f8677710bbb103520 Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Sat, 12 Sep 2026 00:37:12 -0700 Subject: [PATCH 13/18] Add RUN_PROBE entry point; fix stale doc; note sylib dT departure Wire probe_direction to a RUN_PROBE flag via a run_probe helper (scoped allow for the RefCell guard held across the probe's awaits, safe on this path) and drop its dead_code allow. Correct the last "device clock" doc to the Brain's clock, and document that sylib's 5 ms dt quantization is deliberately omitted since V5 motors carry no timestamp. Co-Authored-By: Claude Opus 4.8 --- src/main.rs | 26 ++++++++++++++++++++++++++ src/sensor.rs | 3 +-- src/velocity_estimator.rs | 8 ++++++++ 3 files changed, 35 insertions(+), 2 deletions(-) diff --git a/src/main.rs b/src/main.rs index 74e314d..5351bd9 100644 --- a/src/main.rs +++ b/src/main.rs @@ -28,6 +28,13 @@ 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; + struct Robot { drivetrain: Drivetrain, WheeledTracking>, @@ -107,6 +114,18 @@ impl Compete for Robot { } } +/// Runs the one-shot direction/ticks probe against the first left-side motor. +// The probe holds a `&mut Motor` across its internal awaits, so the `RefCell` +// guard must live that long too. Nothing else borrows these cells on the probe +// path, so the hold is safe; the lint doesn't know that. +#[allow(clippy::await_holding_refcell_ref)] +async fn run_probe(left: &Rc>>) { + let mut motors = left.borrow_mut(); + if let Some(motor) = motors.as_mut().first_mut() { + let _ = sensor::probe_direction(motor).await; + } +} + #[vexide::main] async fn main(peripherals: Peripherals) { let forwards_enc = AdiOpticalEncoder::new(peripherals.adi_a, peripherals.adi_b); @@ -128,6 +147,13 @@ async fn main(peripherals: Peripherals) { let left: Rc>> = Rc::new(RefCell::new(left_motors)); let right: Rc>> = Rc::new(RefCell::new(right_motors)); + // Hardware diagnostic: probe one motor for the direction/ticks constants, + // then exit. No drivetrain model or IMU needed. + if RUN_PROBE { + run_probe(&left).await; + return; + } + // System-identification collector: raw-voltage staircase, no drivetrain // model or IMU needed. Runs to completion, prints Desmos lists, then exits. if RUN_SYSID { diff --git a/src/sensor.rs b/src/sensor.rs index 1ec3c44..caeff6a 100644 --- a/src/sensor.rs +++ b/src/sensor.rs @@ -27,7 +27,7 @@ pub const MOTOR_RAW_POSITION_RESPECTS_DIRECTION: bool = true; /// reading. pub trait TimestampedPosition { type Error; - /// Returns (raw encoder ticks, device clock reading in milliseconds). + /// Returns (raw encoder ticks, the Brain's clock reading in milliseconds). fn timestamped_position(&self) -> Result<(i32, u32), Self::Error>; } @@ -65,7 +65,6 @@ impl TimestampedPosition for Motor { /// 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. -#[allow(dead_code)] pub async fn probe_direction(motor: &mut Motor) -> Result<(), PortError> { let original = motor.direction()?; motor.set_direction(Direction::Reverse)?; diff --git a/src/velocity_estimator.rs b/src/velocity_estimator.rs index 8c72b9c..7e4aa1d 100644 --- a/src/velocity_estimator.rs +++ b/src/velocity_estimator.rs @@ -7,6 +7,14 @@ //! 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). From ec199e27cff290cfe197e3af212aa0451b8b627f Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Sat, 12 Sep 2026 00:40:20 -0700 Subject: [PATCH 14/18] Run probe before sharing motors; drop stale dead_code allow Move the RUN_PROBE branch above the Rc wrapping so the probe borrows left_motors[0] directly, eliminating the borrow-across-await and its clippy allow instead of relying on a comment to stay safe. Remove the now-stale dead_code allow on drive_and_measure. Co-Authored-By: Claude Opus 4.8 --- src/main.rs | 29 +++++++++-------------------- src/sensor.rs | 1 - 2 files changed, 9 insertions(+), 21 deletions(-) diff --git a/src/main.rs b/src/main.rs index 5351bd9..5d68d55 100644 --- a/src/main.rs +++ b/src/main.rs @@ -114,23 +114,11 @@ impl Compete for Robot { } } -/// Runs the one-shot direction/ticks probe against the first left-side motor. -// The probe holds a `&mut Motor` across its internal awaits, so the `RefCell` -// guard must live that long too. Nothing else borrows these cells on the probe -// path, so the hold is safe; the lint doesn't know that. -#[allow(clippy::await_holding_refcell_ref)] -async fn run_probe(left: &Rc>>) { - let mut motors = left.borrow_mut(); - if let Some(motor) = motors.as_mut().first_mut() { - let _ = sensor::probe_direction(motor).await; - } -} - #[vexide::main] 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 left_motors = [ + 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), @@ -141,19 +129,20 @@ async fn main(peripherals: Peripherals) { Motor::new(peripherals.port_19, Gearset::Blue, 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)); - // Hardware diagnostic: probe one motor for the direction/ticks constants, - // then exit. No drivetrain model or IMU needed. - if RUN_PROBE { - run_probe(&left).await; - return; - } - // System-identification collector: raw-voltage staircase, no drivetrain // model or IMU needed. Runs to completion, prints Desmos lists, then exits. if RUN_SYSID { diff --git a/src/sensor.rs b/src/sensor.rs index caeff6a..1f30662 100644 --- a/src/sensor.rs +++ b/src/sensor.rs @@ -105,7 +105,6 @@ pub async fn probe_direction(motor: &mut Motor) -> Result<(), PortError> { /// 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. -#[allow(dead_code)] async fn drive_and_measure(motor: &mut Motor) -> Result<(i32, f64), PortError> { let raw_start = motor.raw_position()?; let pos_start = motor.position()?; From 7807511cfb0cef3bb767c86cfb570a5861e4bf02 Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Sat, 12 Sep 2026 00:42:49 -0700 Subject: [PATCH 15/18] Align with_sources ownership with new Take pre-wrapped Rc>> values in with_sources instead of generic L/R that wrap internally, so callers building motors the way main.rs does can pass them directly. Co-Authored-By: Claude Opus 4.8 --- src/velocity_differential.rs | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/src/velocity_differential.rs b/src/velocity_differential.rs index 010bdf7..de9a29b 100644 --- a/src/velocity_differential.rs +++ b/src/velocity_differential.rs @@ -143,20 +143,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, From 0285633992e59befede1b8de1d641c268a052105 Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Sat, 12 Sep 2026 00:51:37 -0700 Subject: [PATCH 16/18] added more constants and comments to account for the magic numbers in the code emperically tuned from sylib --- src/velocity_estimator.rs | 48 +++++++++++++++++++++++++++++++++------ 1 file changed, 41 insertions(+), 7 deletions(-) diff --git a/src/velocity_estimator.rs b/src/velocity_estimator.rs index 7e4aa1d..3bd72f2 100644 --- a/src/velocity_estimator.rs +++ b/src/velocity_estimator.rs @@ -60,6 +60,38 @@ const INTERNAL_FREE_SPEED_RPM: f64 = 3600.0; /// 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 { @@ -145,13 +177,15 @@ impl VelocityEstimator { // 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: floors at ~0.01 (peak == 0 gives 0.75*(1 - 1/1.013)) - // when steady, rising toward 0.75 during acceleration transients so the - // estimate keeps up. The low floor means slow steady-state settling — - // ~100 samples (~1 s at the motor's ~10 ms data interval); deliberate - // heavy smoothing, so account for it in feedback tuning. - let gain = 0.75 * (1.0 - 1.0 / ((peak * peak / 50.0) + 1.013)); + 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 = From fccaf469c082068a409796524e213490a69256bc Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Sat, 12 Sep 2026 00:58:28 -0700 Subject: [PATCH 17/18] Zero remaining placeholder values in main and flag them with TODOs Zero the demonstration autonomous path (distances, headings, points, per-call overrides) and the WheeledTracking starting pose and wheel geometry, each behind a TODO, so nothing ships with a stray magic number. Co-Authored-By: Claude Opus 4.8 --- src/main.rs | 51 ++++++++++++++++++++++++++++++++------------------- 1 file changed, 32 insertions(+), 19 deletions(-) diff --git a/src/main.rs b/src/main.rs index 5d68d55..1c6e3c9 100644 --- a/src/main.rs +++ b/src/main.rs @@ -42,20 +42,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; } @@ -76,21 +84,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; } @@ -185,11 +196,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), ), ), From e5108b00870d84f3798cf9cdbeadf44ebb67e759 Mon Sep 17 00:00:00 2001 From: Serrial Error Date: Sat, 12 Sep 2026 01:08:16 -0700 Subject: [PATCH 18/18] Pass drive gearset in instead of reading it per motor motor.gearset() fails while a device is still enumerating at power-on, silently mis-scaling that motor's velocity with no log trace. Add a crate-level DRIVE_GEARSET constant, thread it through VelocityDifferential::new and sysid::collect to MotorVelocityTracker::new, and seed every estimator from it instead of a per-motor read with a 600 RPM fallback. Co-Authored-By: Claude Opus 4.8 --- src/main.rs | 19 +++++++++++++------ src/motor_velocity.rs | 35 +++++++++++++++++------------------ src/sysid.rs | 10 +++++++--- src/velocity_differential.rs | 13 +++++++++---- 4 files changed, 46 insertions(+), 31 deletions(-) diff --git a/src/main.rs b/src/main.rs index 1c6e3c9..79d842a 100644 --- a/src/main.rs +++ b/src/main.rs @@ -35,6 +35,11 @@ const RUN_SYSID: bool = false; /// 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>, @@ -130,14 +135,14 @@ 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 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), + 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, @@ -160,6 +165,7 @@ async fn main(peripherals: Peripherals) { sysid::collect( 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` @@ -183,6 +189,7 @@ async fn main(peripherals: Peripherals) { // 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. diff --git a/src/motor_velocity.rs b/src/motor_velocity.rs index 5ca7efe..088ab88 100644 --- a/src/motor_velocity.rs +++ b/src/motor_velocity.rs @@ -24,7 +24,10 @@ use std::{cell::RefCell, f64::consts::PI, rc::Rc}; use vexide::{ math::Direction, prelude::sleep, - smart::{motor::Motor, SmartDevice}, + smart::{ + motor::{Gearset, Motor}, + SmartDevice, + }, task::Task, }; @@ -33,10 +36,6 @@ use crate::{ velocity_estimator::VelocityEstimator, }; -/// Fallback output-shaft free speed (blue cartridge) used if a motor's gearset -/// can't be read while building its estimator. -const DEFAULT_GEARSET_RPM: f64 = 600.0; - /// 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 @@ -71,19 +70,19 @@ pub struct MotorVelocityTracker { impl MotorVelocityTracker { /// Spawns the tracking task over the shared `motors`. Sample order matches /// the motor slice order. - pub fn new(motors: Rc>>) -> Self { - // Build one estimator per motor, seeding each with its own gearset speed. - let mut estimators = Vec::new(); - { - let mut borrow = motors.borrow_mut(); - for motor in borrow.as_mut().iter() { - let gearset_rpm = motor - .gearset() - .map(|gearset| gearset.max_rpm()) - .unwrap_or(DEFAULT_GEARSET_RPM); - estimators.push(VelocityEstimator::new(gearset_rpm)); - } - } + /// + /// `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()])); diff --git a/src/sysid.rs b/src/sysid.rs index 327fc36..a364ddf 100644 --- a/src/sysid.rs +++ b/src/sysid.rs @@ -62,7 +62,10 @@ use std::{ 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}; @@ -127,14 +130,15 @@ struct Step { 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()); - let right_tracker = MotorVelocityTracker::new(right.clone()); + let left_tracker = MotorVelocityTracker::new(left.clone(), gearset); + let right_tracker = MotorVelocityTracker::new(right.clone(), gearset); let mut steps = Vec::new(); diff --git a/src/velocity_differential.rs b/src/velocity_differential.rs index de9a29b..f05ca0e 100644 --- a/src/velocity_differential.rs +++ b/src/velocity_differential.rs @@ -34,7 +34,10 @@ 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}; @@ -111,20 +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: Rc>>, right: Rc>>, gear_ratio: f64, + gearset: Gearset, config: VelocityDifferentialConfig, ) -> 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()), gear_ratio); + MotorGroupVelocity::new(MotorVelocityTracker::new(left.clone(), gearset), gear_ratio); let right_source = - MotorGroupVelocity::new(MotorVelocityTracker::new(right.clone()), gear_ratio); + MotorGroupVelocity::new(MotorVelocityTracker::new(right.clone(), gearset), gear_ratio); Self { left,