Fire control SOTM solver with drag compensation and confidence scoring

[dmeglan]

Summary

Replaces the inline fixed-point SOTM iteration in RobotState.periodic() with a proper Newton-Raphson fire control solver, based on Team 5962's open-source fire control system (Chief Delphi thread).

Why This Change Is Needed

The current SOTM implementation has five significant gaps compared to the reference fire control system:

1. No drag compensation on horizontal ball travel

Current: effectiveLocation = target - v * shotTime (linear)

New: Uses exponential decay: driftTOF = (1 - e^(-c*t)) / c

Air resistance decays the ball's inherited horizontal velocity exponentially. At a drag coefficient of 0.24 and 0.8s flight time, the effective drift is ~0.70s instead of 0.80s — meaning the current code over-compensates by ~12% at longer distances. The ball doesn't drift as far as the linear model predicts because air resistance slows down its inherited sideways velocity during flight.

2. No latency compensation

Current: Uses the robot's instantaneous pose and velocity.

New: Predicts the robot pose forward by vision_latency + mechanism_latency (~50ms) using first-order kinematics.

At 3 m/s robot speed, 50ms of uncompensated latency = 15cm of position error. The robot has moved since the vision frame was captured, and the turret/hood/flywheel take time to respond to new setpoints. By predicting forward, the solver aims at where the robot will be when the mechanisms settle, not where it was when the camera saw the target.

3. No warm start

Current: Starts from raw distance each cycle, runs a fixed number of iterations.

New: Reuses previous cycle's TOF as initial guess for the Newton solver.

Since the robot's state changes smoothly between 20ms cycles, the previous TOF is almost always very close to the new solution. This allows convergence in 2-3 iterations instead of 5+, and prevents oscillation in edge cases where the fixed-point iteration can bounce between two values.

4. No confidence scoring

Current: Binary readyToShoot() — either all mechanisms are at target, or not.

New: 0-100 weighted geometric mean of four components:

• Solver convergence (did Newton converge? how many iterations?)
• Velocity stability (penalizes rapid acceleration/deceleration)
• Heading accuracy (how well-aligned is the turret?)
• Distance in range (penalty near min/max scoring boundaries)

One zero in any component kills the entire score. This prevents shooting during transient states (e.g., sudden direction changes, turret catching up, robot at extreme range).

The confidence threshold is configurable via kMinShootConfidence (default 30). readyToShoot() now requires sotmConfidence >= kMinShootConfidence in addition to the existing mechanism checks.

5. Fixed-point iteration vs Newton-Raphson

Current: Simple substitution loop — compute distance, look up TOF, shift target, repeat.

New: Newton's method with analytical derivatives:

f(t) = g(d(t)) - t    (lookup TOF at projected distance, minus current TOF guess)
f'(t) = g'(d) * d'(t) - 1

t_new = t - f(t) / f'(t)

Where d'(t) includes the drag derivative e^(-ct) and g'(d) is the numerical derivative of the TOF lookup. Newton's method converges quadratically vs linearly for fixed-point iteration, meaning it needs far fewer iterations for the same precision.

Architecture

Files Changed

File	Change
src/util/firecontrol.py	New — FireControlSolver, FireControlConfig, FireControlResult
src/constants/shooting.py	Added kFireControlConfig and kMinShootConfidence
src/robotstate.py	Replaced inline SOTM loop with solver call, added confidence to readyToShoot()

Drop-in Replacement

The solver populates the same RobotState.effectiveObjectiveLocation and RobotState.effectiveObjectiveDistance class variables that the turret/shooting/hood commands already read. No command code changes needed. The existing trackedTurretMoving, shootBallsMoving, feedBallsMoving, and angleHoodWithDistance all work unchanged.

FireControlConfig Parameters

FireControlConfig(
    max_iterations=10,          # Newton solver max iterations
    convergence_tolerance=0.001, # seconds — stop when TOF changes less than this
    sotm_drag_coeff=0.24,       # 1/s — exponential drag decay rate (0 = disabled)
    min_sotm_speed=0.1,         # m/s — below this, static aiming (no compensation)
    vision_latency=0.030,       # seconds — vision pipeline delay
    mechanism_latency=0.020,    # seconds — turret/hood/flywheel response time
    min_scoring_distance=1.0,   # meters — confidence degrades below this
    max_scoring_distance=6.0,   # meters — confidence degrades above this
)

Logging

The solver logs to AdvantageScope under Robot/SOTM/:

• TOF — converged time of flight (seconds)
• Confidence — shot confidence (0-100)
• Iterations — Newton iterations used this cycle
• IsStatic — true if robot was below speed threshold
• Converged — true if solver converged within tolerance

Tuning Guide

1. sotm_drag_coeff — Start at 0.24. Increase if shots land short of target when moving (ball drifts less than expected). Decrease or set to 0 if shots overshoot. This is the most impactful tuning parameter.

2. kMinShootConfidence — Start at 30. Increase if the robot fires bad shots during transients. Decrease if the robot won't fire when it should. Watch Robot/SOTM/Confidence in AdvantageScope.

3. vision_latency / mechanism_latency — Measure with AdvantageScope by comparing vision timestamp vs robot pose timestamp. Default 30ms + 20ms is conservative.

4. min_sotm_speed — Below this speed, the solver skips SOTM entirely and uses direct distance. Default 0.1 m/s (essentially stationary).

Reference

• Team 5962 Fire Control Repository (MIT License)
• Chief Delphi Discussion

The solver algorithm is adapted from ShotCalculator.java in the reference repo, translated to Python and integrated with our existing RobotState architecture and interpolation maps.

Test Plan

• Deploy and verify Robot/SOTM/Confidence appears in AdvantageScope
• Verify static shooting still works (robot stationary, confidence should be high)
• Test SOTM at low speed (~1 m/s) — compare shot accuracy vs previous implementation
• Test SOTM at high speed (~3 m/s) — check that confidence drops prevent bad shots
• Tune sotm_drag_coeff based on shot accuracy at different speeds/distances
• Adjust kMinShootConfidence threshold based on match play experience
• Verify readyToShoot() gates properly with confidence (robot shouldn't fire with low confidence)

[3arc7]

Eager to see how this works; tuning might be tricky.

[lmaxwell24]

for simplicity please also use the LogTunableNumber for ease of tuning "magic values" that we will want to modify

[lmaxwell24]

if using wpistruct, this type can be logged directly. See vision subsystem for how to transform a dataclass into a pykit compatible logging structure

[lmaxwell24]

these are a lot of parameters that the LogTunableNumber class can be useful for tuning. See drivewaypoint.py for example usage

[lmaxwell24]

a "bad" SOTM solve prevents shooting but has no existing indicator of why specifically a shot request was denied. Please revisit a way of on the fly debugging if the shot isn't "up to par"