OTF shooting: fix interpolation, convergence, and loop performance by phurley67 · Pull Request #536 · frc-862/mirage

phurley67 · 2026-04-06T01:48:07Z

Closes #535

Summary

SplineMap for non-monotonic TOF data -- replaces ThunderMap (linear interpolation) with cubic spline interpolation for the time-of-flight lookup. Linear interpolation gives wrong values between the non-monotonic calibration points (228" high-arc shot takes longer than 262" flat shot). Spline fits smooth curves through all data points.
Under-relaxation fixes convergence -- the OTF fixed-point iteration diverges when |speed * dTOF/dDist| > 1, which happens above ~3.3 m/s with the spline slopes. Added 0.5 relaxation factor so each iteration blends halfway toward the prediction, keeping the loop stable at any FRC speed.
Zero-allocation loop -- rewrote the OTF convergence loop to use raw double arithmetic instead of Pose2d/Measure objects. Allocations only happen at the end when setting motor outputs. SplineMap.getBaseUnit() provides a no-allocation lookup path. Estimated: ~3500 objects/sec eliminated.
build.gradle test fix -- added useJUnitPlatform() so ./gradlew test actually discovers and runs JUnit 5 tests.

Test plan

Run ./gradlew test --tests "frc.util.SplineMapTest" -- all 4 tests pass, check the printed interpolation table
Deploy to robot, drive at various speeds while OTF shooting -- verify shots land closer than before
At high speed (>3 m/s), verify OTF doesn't oscillate (watch "Future Pose" on Shuffleboard)
Verify canned shots and static shooting still work (no regressions)

🤖 Generated with Claude Code

Add SplineMap for cubic spline interpolation of non-monotonic TOF data. Add under-relaxation to guarantee convergence at high robot speeds. Rewrite OTF loop to use raw doubles, eliminating GC pressure. Fix build.gradle to actually discover and run JUnit 5 tests. Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

Copilot

Pull request overview

This PR improves on-the-fly (OTF) shooting accuracy and stability by replacing linear TOF interpolation with cubic splines, stabilizing the fixed-point iteration with under-relaxation, and reducing per-loop allocations. It also updates Gradle test configuration to ensure JUnit 5 tests are discovered and run.

Changes:

Added SplineMap (natural cubic spline) as a drop-in replacement for linear TOF lookup.
Reworked Cannon.shootOTF() to use an under-relaxed, zero-allocation convergence loop and increased max iterations.
Updated build.gradle to enable JUnit 5 discovery via useJUnitPlatform() and added a new SplineMapTest.

Reviewed changes

Copilot reviewed 3 out of 4 changed files in this pull request and generated 5 comments.

File	Description
src/main/java/frc/util/units/SplineMap.java	New natural cubic spline interpolating map with a base-unit zero-allocation lookup path.
src/main/java/frc/robot/subsystems/Cannon.java	Switches TOF lookup to `SplineMap` and rewrites OTF convergence loop for stability/perf.
src/test/java/frc/util/SplineMapTest.java	Adds tests comparing spline vs linear interpolation and clamping behavior.
build.gradle	Enables JUnit 5 test discovery with `useJUnitPlatform()`.

Copilot · 2026-04-06T01:52:13Z

 import frc.util.shuffleboard.LightningShuffleboard;
+import frc.util.units.SplineMap;
 import frc.util.units.ThunderMap;


Unused import frc.util.units.ThunderMap will cause a Java compile error (imports must be used). Remove this import or use it in this file (it looks like Cannon now only uses SplineMap and other maps).

Copilot · 2026-04-06T01:52:13Z

+import org.junit.jupiter.api.Test;
+
+import static edu.wpi.first.units.Units.Inches;
+import static edu.wpi.first.units.Units.Meters;


Unused static import Meters will cause a Java compile error. Remove it or use it in this test file.

Suggested change

import static edu.wpi.first.units.Units.Meters;

Copilot · 2026-04-06T01:52:13Z

+    @Test
+    void printInterpolationComparison() {
+        // Print a comparison table so students can visualize the difference
+        System.out.println("\nDistance(in) | Linear TOF(s) | Spline TOF(s) | Diff(s)");
+        System.out.println("------------|---------------|---------------|--------");


printInterpolationComparison() is annotated with @Test but contains no assertions and prints a large table to stdout, which can make CI logs noisy while not validating behavior. Consider converting this to an assertion-based test, marking it @Disabled, or moving it to a non-test helper (e.g., a main method or a locally-run debug tool).

Copilot · 2026-04-06T01:52:13Z

+            // then rotated to field frame and added to chassis velocity.
+            // This matches getFuturePoseFromTime's existing math.


The comment says this velocity math “matches getFuturePoseFromTime's existing math”, but Swerve.getFuturePoseFromTime() currently computes the shooter-offset rotational terms differently (see Swerve.java:441-451). Either update this comment to reflect the intentional behavior change, or refactor to share a single implementation so OTF and getFuturePoseFromTime() stay consistent.

Suggested change

// then rotated to field frame and added to chassis velocity.

// This matches getFuturePoseFromTime's existing math.

// then rotated to field frame and added to chassis velocity for OTF aiming.

// Keep this description local to this implementation; getFuturePoseFromTime()

// currently computes its shooter-offset rotational terms separately.

Copilot · 2026-04-06T01:52:14Z

+            // Robot-relative velocity of the shooter offset due to chassis rotation,
+            // then rotated to field frame and added to chassis velocity.
+            // This matches getFuturePoseFromTime's existing math.
+            double rrXVel = -speeds.omegaRadiansPerSecond * shooterOffY;
+            double rrYVel =  speeds.omegaRadiansPerSecond * shooterOffX;
+            double totalVx = speeds.vxMetersPerSecond + (rrXVel * cos - rrYVel * sin);
+            double totalVy = speeds.vyMetersPerSecond + (rrXVel * sin + rrYVel * cos);


totalVx/totalVy appear to mix coordinate frames: speeds.vx/vy are used elsewhere as robot-relative (e.g., getHubAngularVelocity() rotates them by the robot heading), but here the shooter-offset rotational velocity is rotated by cos/sin before being added. This likely double-rotates part of the motion when you later rotate (twistDx, twistDy) into the field frame. Consider keeping all velocities robot-relative (add rrXVel/rrYVel directly to speeds.vx/vy) and only rotate once when converting the twist/translation into field coordinates, or alternatively convert chassis speeds to field-relative up front and then avoid the later rotation.

Suggested change

// Robot-relative velocity of the shooter offset due to chassis rotation,

// then rotated to field frame and added to chassis velocity.

// This matches getFuturePoseFromTime's existing math.

double rrXVel = -speeds.omegaRadiansPerSecond * shooterOffY;

double rrYVel = speeds.omegaRadiansPerSecond * shooterOffX;

double totalVx = speeds.vxMetersPerSecond + (rrXVel * cos - rrYVel * sin);

double totalVy = speeds.vyMetersPerSecond + (rrXVel * sin + rrYVel * cos);

// Robot-relative velocity of the shooter offset due to chassis rotation.

// Keep this in the robot frame when combining with chassis velocity so the

// translation/twist math below only rotates into the field frame once.

double rrXVel = -speeds.omegaRadiansPerSecond * shooterOffY;

double rrYVel = speeds.omegaRadiansPerSecond * shooterOffX;

double totalVx = speeds.vxMetersPerSecond + rrXVel;

double totalVy = speeds.vyMetersPerSecond + rrYVel;

@test

…, and build.gradle test fix Three improvements integrated from PR #536 (fix/otf-spline-convergence): 1. SplineMap: Replaces ThunderMap (linear interpolation) with natural cubic spline interpolation for the TIME_OF_FLIGHT_MAP. Our TOF data is non- monotonic (186" = 1.51s, 228" = 1.40s), and linear interpolation draws straight lines through the peak, missing the true curve shape. The spline captures it correctly. Drop-in replacement with identical put/get API. 2. Under-relaxation: The OTF fixed-point iteration can diverge when |speed * dTOF/dDistance| > 1, which happens at ~3.3 m/s with the spline's steeper slopes. Each iteration now blends partway toward the new prediction (RELAXATION = 0.5), guaranteeing convergence at any FRC speed. MAX_OTF_ITERATIONS bumped from 10 to 15 as a safety margin, though warm-starting keeps actual iterations at 2-3. 3. build.gradle: Added useJUnitPlatform() so JUnit 5 tests actually run. Without this, Gradle silently skipped all @test methods. Also adds Future Pose Shuffleboard logging for OTF aim-point debugging. Credits: SplineMap, under-relaxation, and build fix originated in PR #536. Closes #548 Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

Copilot AI review requested due to automatic review settings April 6, 2026 01:48

Copilot started reviewing on behalf of phurley67 April 6, 2026 01:48 View session

phurley67 mentioned this pull request Apr 6, 2026

OTF: account for robot rotation and use future-pose hub velocity #538

Open

5 tasks

Copilot AI reviewed Apr 6, 2026

View reviewed changes

phurley67 mentioned this pull request Apr 7, 2026

SplineMap for TOF interpolation, under-relaxation for convergence, and build.gradle test fix #548

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

OTF shooting: fix interpolation, convergence, and loop performance#536

OTF shooting: fix interpolation, convergence, and loop performance#536
phurley67 wants to merge 1 commit into
mainfrom
fix/otf-spline-convergence

phurley67 commented Apr 6, 2026

Uh oh!

Copilot AI left a comment

Uh oh!

Copilot AI Apr 6, 2026

Uh oh!

Copilot AI Apr 6, 2026

Uh oh!

Copilot AI Apr 6, 2026

Uh oh!

Copilot AI Apr 6, 2026

Uh oh!

Copilot AI Apr 6, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

		// then rotated to field frame and added to chassis velocity.
		// This matches getFuturePoseFromTime's existing math.

-            // then rotated to field frame and added to chassis velocity.
-            // This matches getFuturePoseFromTime's existing math.
+            // then rotated to field frame and added to chassis velocity for OTF aiming.
+            // Keep this description local to this implementation; getFuturePoseFromTime()
+            // currently computes its shooter-offset rotational terms separately.

Conversation

phurley67 commented Apr 6, 2026

Summary

Test plan

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull request overview

Reviewed changes

Uh oh!

Copilot AI Apr 6, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 6, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 6, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 6, 2026

Choose a reason for hiding this comment

Uh oh!

Copilot AI Apr 6, 2026

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants