From da0a2b476e097f9fcdfa3b64a456c88ef600c982 Mon Sep 17 00:00:00 2001 From: Pixel Raptors FTC 28395 Date: Sat, 19 Sep 2026 00:30:11 -0700 Subject: [PATCH] Stop the Foresight tuner on NaN results and runaways A failed system-identification fit could hand NaN to the later steps: with fewer than two samples between 10% and 80% of the steady speed, linearFit returns a NaN slope, which the `== 0` check lets through. A NaN heading kP makes every power the braking steps send NaN, CachedMotor ignores NaN powers, and the motors keep their last power, so the robot drives off until someone stops it. - The three fits fail on fewer than two samples, or a NaN or wrong-sign slope. - Each step runs through step(): a failure stops the tuning with the step's name and reason. - Values the later steps drive with, and everything in the generated config, are checked: NaN, infinite, or not positive where they must be stops the tuning with a message. - The braking steps stop the robot if it gets 24 in past the test's ends or drifts 48 in across them. --- .../pedro/procedures/ForesightTuner.java | 104 +++++++++++++++--- 1 file changed, 89 insertions(+), 15 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ForesightTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ForesightTuner.java index f1f5e8693229..22f8a1abc57c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ForesightTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedro/procedures/ForesightTuner.java @@ -35,37 +35,38 @@ public void run() throws InterruptedException { Inputs.Field distance = distanceInput.d("Distance").withDefault(48.0); awaitInputs(distanceInput); - double forwardVelocity = runOpMode(new ForwardVelocity(localizerFunction, drivetrainFunction, distance.get())); - double strafeVelocity = runOpMode(new StrafeVelocity(localizerFunction, drivetrainFunction, distance.get())); + double forwardVelocity = step(new ForwardVelocity(localizerFunction, drivetrainFunction, distance.get())); + double strafeVelocity = step(new StrafeVelocity(localizerFunction, drivetrainFunction, distance.get())); Inputs velocityInput = inputs("Velocity", "The velocity to drive to in inches for the Max Achievable Forward and Strafe Deceleration Identifiers"); Inputs.Field velocity = velocityInput.d("Velocity").withDefault(30.0); awaitInputs(velocityInput); - double forwardDeceleration = runOpMode(new ForwardDeceleration(localizerFunction, drivetrainFunction, velocity.get())); - double strafeDeceleration = runOpMode(new StrafeDeceleration(localizerFunction, drivetrainFunction, velocity.get())); + double forwardDeceleration = step(new ForwardDeceleration(localizerFunction, drivetrainFunction, velocity.get())); + double strafeDeceleration = step(new StrafeDeceleration(localizerFunction, drivetrainFunction, velocity.get())); - List headingBraking = runOpMode(new HeadingBraking(localizerFunction, drivetrainFunction)); - double heading = runOpMode(new HeadingTuner(localizerFunction, drivetrainFunction)); + List headingBraking = step(new HeadingBraking(localizerFunction, drivetrainFunction)); + double heading = step(new HeadingTuner(localizerFunction, drivetrainFunction)); - double headingLinear = headingBraking.get(0); - double headingQuadratic = headingBraking.get(1); + heading = measured("Heading Tuner", "heading kP", heading, true); + double headingLinear = measured("Heading Braking", "the linear coefficient", headingBraking.get(0), false); + double headingQuadratic = measured("Heading Braking", "the quadratic coefficient", headingBraking.get(1), false); Inputs distanceBrakingInput = inputs("Distance", "The distance to drive in inches for the Forward and Strafe Braking Identifiers. Distance must be at least 15 inches for accurate results."); Inputs.Field distanceBraking = distanceBrakingInput.d("Distance").withDefault(36.0); awaitInputs(distanceBrakingInput); double safeDistanceBraking = Math.max(distanceBraking.get(), 15.0); - List forwardBraking = runOpMode(new ForwardBraking(localizerFunction, drivetrainFunction, headingLinear, headingQuadratic, heading, safeDistanceBraking)); - List strafeBraking = runOpMode(new StrafeBraking(localizerFunction, drivetrainFunction, headingLinear, headingQuadratic, heading, safeDistanceBraking)); + List forwardBraking = step(new ForwardBraking(localizerFunction, drivetrainFunction, headingLinear, headingQuadratic, heading, safeDistanceBraking)); + List strafeBraking = step(new StrafeBraking(localizerFunction, drivetrainFunction, headingLinear, headingQuadratic, heading, safeDistanceBraking)); double forwardLinear = forwardBraking.get(0); double forwardQuadratic = forwardBraking.get(1); double strafeLinear = strafeBraking.get(0); double strafeQuadratic = strafeBraking.get(1); - List forwardTranslational = runOpMode(new ForwardTranslational(localizerFunction, drivetrainFunction)); - List strafeTranslational = runOpMode(new StrafeTranslational(localizerFunction, drivetrainFunction)); + List forwardTranslational = step(new ForwardTranslational(localizerFunction, drivetrainFunction)); + List strafeTranslational = step(new StrafeTranslational(localizerFunction, drivetrainFunction)); double forwardTranslationalPrimary = forwardTranslational.get(0); double forwardTranslationalSecondary = forwardTranslational.get(1); @@ -75,6 +76,21 @@ public void run() throws InterruptedException { double strafeTranslationalPrimary = strafeTranslational.get(0); double strafeTranslationalSecondary = strafeTranslational.get(1); + measured("Max Forward Velocity", "the max forward velocity", forwardVelocity, true); + measured("Max Strafe Velocity", "the max strafe velocity", strafeVelocity, true); + measured("Forward Deceleration", "the natural forward deceleration", forwardDeceleration, true); + measured("Strafe Deceleration", "the natural strafe deceleration", strafeDeceleration, true); + measured("Forward Braking", "the linear coefficient", forwardLinear, false); + measured("Forward Braking", "the quadratic coefficient", forwardQuadratic, false); + measured("Strafe Braking", "the linear coefficient", strafeLinear, false); + measured("Strafe Braking", "the quadratic coefficient", strafeQuadratic, false); + measured("Forward Translational", "the primary kP", forwardTranslationalPrimary, true); + measured("Forward Translational", "the secondary kP", forwardTranslationalSecondary, true); + measured("Strafe Translational", "the primary kP", strafeTranslationalPrimary, true); + measured("Strafe Translational", "the secondary kP", strafeTranslationalSecondary, true); + measured("Forward Translational", "coast kV", coast, false); + measured("Forward Translational", "brake kV", brake, false); + result("maxAchievableForwardVelocity", forwardVelocity); result("maxAchievableStrafeVelocity", strafeVelocity); result("naturalForwardDeceleration", forwardDeceleration); @@ -120,6 +136,32 @@ public void run() throws InterruptedException { " }\n" + " );"); } + + /** + * Runs one step. If it fails, the tuning stops with the step's name and reason instead of the + * exception ending the procedure without a message. + */ + private T step(TuningOpMode opMode) throws InterruptedException { + try { + return runOpMode(opMode); + } catch (RuntimeException e) { + abort(opMode.name + " failed: " + e.getMessage()); + throw e; // not reached: abort() throws + } + } + + /** + * A measured value that later steps drive with, or that goes into the generated config. Stops the + * tuning if it is NaN or infinite, or not positive where it must be. A NaN heading kP, for example, + * made every power the braking steps sent NaN; CachedMotor ignores NaN powers, so the motors kept + * their last power and the robot drove off. + */ + private double measured(String step, String name, double value, boolean positive) throws InterruptedException { + if (!Double.isFinite(value) || (positive && value <= 0)) { + abort(step + " measured " + name + " = " + value + ", which can't be used. Run the tuner again."); + } + return value; + } } class ForwardVelocity extends TuningOpMode { @@ -673,7 +715,11 @@ private void systemIdentification() { x.toArray(new Double[0]), y.toArray(new Double[0]) ); - if (linReg[1] == 0) throw new IllegalArgumentException("Failed calibration."); + // With fewer than two samples between 10% and 80% the slope is NaN, which `== 0` let through. + if (x.size() < 2 || !Double.isFinite(linReg[1]) || linReg[1] >= 0) { + throw new IllegalArgumentException("Failed calibration: only " + x.size() + + " samples while the robot sped up."); + } this.tau = -1.0/linReg[1]; } } @@ -694,6 +740,10 @@ class ForwardBraking extends TuningOpMode> { public double brakingPower = 0.001; public double distance; public double IDLE_SECONDS = 1; + /** Stops the robot if it gets this many inches past the test's ends. */ + public double OVERRUN = 24; + /** Stops the robot if it drifts this many inches across the test's direction. */ + public double DRIFT = 48; private final ElapsedTime timer = new ElapsedTime(); private final List velocityToBrakingDistance = new ArrayList<>(); @@ -737,6 +787,12 @@ protected List runTuningOpMode() throws InterruptedException { while (state != State.DONE && !isStopRequested()) { localizer.update(); + if (Math.abs(localizer.pose().x()) > distance + OVERRUN || Math.abs(localizer.pose().y()) > DRIFT) { + drivetrain.stop(); + throw new IllegalStateException(String.format(Locale.US, + "stopped the robot at (%.0f, %.0f): more than %.0f in past the test's ends or %.0f in across them", + localizer.pose().x(), localizer.pose().y(), OVERRUN, DRIFT)); + } direction = (iteration % 2 == 0) ? 1 : -1; if (iteration < POWERS.length) { power = POWERS[iteration]; @@ -862,6 +918,10 @@ class StrafeBraking extends TuningOpMode> { public double brakingPower = 0.001; public double distance; public double IDLE_SECONDS = 1; + /** Stops the robot if it gets this many inches past the test's ends. */ + public double OVERRUN = 24; + /** Stops the robot if it drifts this many inches across the test's direction. */ + public double DRIFT = 48; private final ElapsedTime timer = new ElapsedTime(); private final List velocityToBrakingDistance = new ArrayList<>(); @@ -905,6 +965,12 @@ protected List runTuningOpMode() throws InterruptedException { while (state != State.DONE && !isStopRequested()) { localizer.update(); + if (Math.abs(localizer.pose().y()) > distance + OVERRUN || Math.abs(localizer.pose().x()) > DRIFT) { + drivetrain.stop(); + throw new IllegalStateException(String.format(Locale.US, + "stopped the robot at (%.0f, %.0f): more than %.0f in past the test's ends or %.0f in across them", + localizer.pose().x(), localizer.pose().y(), OVERRUN, DRIFT)); + } direction = (iteration % 2 == 0) ? 1 : -1; if (iteration < POWERS.length) { power = POWERS[iteration]; @@ -1129,7 +1195,11 @@ private void systemIdentification() { x.toArray(new Double[0]), y.toArray(new Double[0]) ); - if (linReg[1] == 0) throw new IllegalArgumentException("Failed calibration."); + // With fewer than two samples between 10% and 80% the slope is NaN, which `== 0` let through. + if (x.size() < 2 || !Double.isFinite(linReg[1]) || linReg[1] >= 0) { + throw new IllegalArgumentException("Failed calibration: only " + x.size() + + " samples while the robot sped up."); + } this.tau = -1.0/linReg[1]; } } @@ -1246,7 +1316,11 @@ private void systemIdentification() { x.toArray(new Double[0]), y.toArray(new Double[0]) ); - if (linReg[1] == 0) throw new IllegalArgumentException("Failed calibration."); + // With fewer than two samples between 10% and 80% the slope is NaN, which `== 0` let through. + if (x.size() < 2 || !Double.isFinite(linReg[1]) || linReg[1] >= 0) { + throw new IllegalArgumentException("Failed calibration: only " + x.size() + + " samples while the robot sped up."); + } this.tau = -1.0/linReg[1]; } }