From 50b7d81dc9f6ff73436ef93048eb2f37033f6f4c Mon Sep 17 00:00:00 2001 From: Arnav495 Date: Wed, 11 Feb 2026 16:20:25 -0800 Subject: [PATCH] It passes! --- src/main/java/frc/robot/util/ShooterPhysics.java | 15 ++++++++++----- .../java/frc/robot/util/ShooterPhysicsTest.java | 10 +++++----- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index 2589fb4..f207dac 100644 --- a/src/main/java/frc/robot/util/ShooterPhysics.java +++ b/src/main/java/frc/robot/util/ShooterPhysics.java @@ -192,13 +192,14 @@ public class ShooterPhysics { public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target, double tolerance) { // System.out.println("!!! inv:" + initialVelocity + " tgt:" + target + " tlr:" - // + tolerance); + // + tolerance); // trying to calculate a shot for height=0 returns NaN double effectiveMinHeight = Math.max(target.getZ(), 0.01); TurretState first = cvtShot(getRequiredExitVelocity(initialVelocity, target, effectiveMinHeight), effectiveMinHeight); - // if the minimum velocity is below our minimum height, that's the closest we can get + // if the minimum velocity is below our minimum height, that's the closest we + // can get if (getVelocityDiff(first, initialVelocity, target) >= 0) return first; @@ -257,8 +258,11 @@ public class ShooterPhysics { // if a shot requires going up a kilometer, it's probably not doable TurretState second = cvtShot(getRequiredExitVelocity(initialVelocity, target, 1000.), 1000.); - if (first.pitch() > pitch + tolerance) - return Optional.empty(); + if (first.pitch() > pitch) + if (first.pitch() <= pitch + tolerance) + return Optional.of(first); + else + return Optional.empty(); else if (second.pitch() < pitch) return Optional.of(second); // it's close enough @@ -279,7 +283,8 @@ public class ShooterPhysics { return Optional.of(guess); // we've narrowed the range but haven't found a valid angle // should be covered by the checks before the loop - assert false; + throw new RuntimeException( + "Solving for angle resulted in an empty range " + range + " (pitch: " + pitch + ")."); } if (guess.pitch() > pitch) diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java index 5c3572c..f63d598 100644 --- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java +++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java @@ -9,7 +9,6 @@ import java.util.Random; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Disabled; import org.junit.jupiter.api.Test; import edu.wpi.first.math.geometry.Rotation2d; @@ -271,7 +270,6 @@ class ShooterPhysicsTest { } } - @Disabled @Test public void simulatedConstraintsTest() { Random rng = new Random(6328); @@ -294,10 +292,12 @@ class ShooterPhysicsTest { // check going down breaks a constraint; we've found the minimum var lower = ShooterPhysics.withAngle(initVel, robotToTarget, state.get().pitch() - .1); - assertTrue(lower.isEmpty()); + assertTrue(lower.isEmpty() || !lower.get().satisfies(constraints)); // check going up is okay - var higher = ShooterPhysics.withAngle(initVel, robotToTarget, state.get().pitch() + .1); - assertTrue(higher.isPresent()); + if (state.get().pitch() + 0.1 < constraints.maxPitch()) { + var higher = ShooterPhysics.withAngle(initVel, robotToTarget, state.get().pitch() + .1); + assertTrue(higher.isPresent()); + } } } } -- 2.39.5