Add sylib-style motor velocity estimator for drivetrain feedback - #16
Open
SerrialError wants to merge 9 commits into
Open
Add sylib-style motor velocity estimator for drivetrain feedback#16SerrialError wants to merge 9 commits into
SerrialError wants to merge 9 commits into
Conversation
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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
This reverts commit ebe2f5a.
- 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 <noreply@anthropic.com>
- 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 <noreply@anthropic.com>
SerrialError
commented
Sep 8, 2026
Owner
Author
There was a problem hiding this comment.
Get rid of DEFAULT_GEARSET_RPM, require it
…NAL_REV - 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 <noreply@anthropic.com>
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 <noreply@anthropic.com>
…rate - motor_velocity: publish Vec<Option<f64>>. 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<f64>]. - 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 <noreply@anthropic.com>
- sysid: mirror the motor_velocity fix. update_estimators writes None on a failed read (buffer is now Vec<Option<f64>>), 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 <noreply@anthropic.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
What
Replaces
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 velocity-estimation writeup. The V5 motor's built-in velocity estimate is noisy and laggy at the speeds a velocity loop cares about.The pipeline
Per sample (
VelocityEstimator::update):|raw| > 5000RPM) as position resets.New files
src/filters.rs—Sma,Median,MaxAbs,Ema,Derivative. No vexide dependency; unit-tested.src/velocity_estimator.rs—VelocityEstimator. No vexide dependency; unit-tested.src/sensor.rs—TimestampedPositiontrait (raw ticks + device-clock ms), implemented forMotor.src/motor_velocity.rs—MotorVelocityTracker: one estimator per motor on a background task, publishing per-motor RPM into a sharedRc<RefCell<Vec<f64>>>. The sampling borrow is never held across an.await.Changed
src/velocity_differential.rs—MotorGroupVelocityaverages the tracker's shared cell instead of pollingmotor.velocity(); each side gets its own tracker. Thegear_ratio * (2π/60)conversion andWheelVelocityimpl are unchanged.src/sysid.rs— records both estimated and raw omega per sample and emits the raw series as a third Desmos list (z_1), so the filtered estimate can be sanity-checked against the unfiltered one.Testing
filters.rsandvelocity_estimator.rspass (compiled standalone, since the crate proper only links for the V5 target).cargo v5 buildlinks cleanly forarmv7a-vex-v5with no warnings.TODOs left in code (need hardware)
TICKS_PER_INTERNAL_REV = 50.0— verify by spinning one full output revolution and diffingraw_position().MOTOR_RAW_POSITION_RESPECTS_DIRECTION = true— verify; if false, reversed-motor output is negated (already wired).timestamp()is the Brain's packet timestamp, not the motor's own sample time; swap to thevexDeviceMotorPositionRawGetout-param once vexide exposes it (vexide#386).🤖 Generated with Claude Code