From: Arnav495 Date: Sat, 7 Feb 2026 00:38:26 +0000 (-0800) Subject: More tests and bugfixes. X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=5488f2dac068e389b3f7fe2446f7b3bc9e7f3273;p=FRC2026.git More tests and bugfixes. --- diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index d79a19d..dad2995 100644 --- a/src/main/java/frc/robot/util/ShooterPhysics.java +++ b/src/main/java/frc/robot/util/ShooterPhysics.java @@ -44,11 +44,11 @@ public class ShooterPhysics { public static Optional getConstrainedParams(Translation2d robotVelocity, Translation3d robotToTarget, Constraints constraints) { // establish a lower bound - double minHeight = Math.max(robotToTarget.getZ(), constraints.height()); + double minHeight = Math.max(Math.max(robotToTarget.getZ(), constraints.height()), 0.01); Optional withMinPitch = withAngle(robotVelocity, robotToTarget, constraints.minPitch()); - if (withMinPitch.isPresent()) { - minHeight = Math.min(minHeight, withMinPitch.get().height()); - } + if (withMinPitch.isPresent()) + minHeight = Math.max(minHeight, withMinPitch.get().height()); + TurretState withMinHeight = cvtShot(getRequiredExitVelocity(robotVelocity, robotToTarget, minHeight), minHeight); if (withMinHeight.satisfies(constraints)) @@ -162,7 +162,8 @@ public class ShooterPhysics { // this is not equivalent to t <= 0 because of NaNs if (!(t > 0)) - throw new RuntimeException("Time should never be negative (got t=" + t + ")."); + throw new RuntimeException("Time should never be negative (got t=" + t + " with target: " + target + + " and peakZ: " + peakZ + ")."); // calculate x and z exit_vel // x = (v_x_robot + v_x_exit_vel) * t @@ -223,33 +224,53 @@ public class ShooterPhysics { public static Optional withAngle(Translation2d initialVelocity, Translation3d target, double pitch, double tolerance) { - // guess a peak height - double guess = target.getZ() + 2; + if (pitch <= 0 || pitch >= Math.PI / 2) + throw new IllegalArgumentException("Pitch must be in the range 0 < pitch < pi/2 (got: " + pitch + ")."); + + // System.out.println( + // "Solving for pitch=" + pitch + " with target=" + target + " and + // initialVelocity=" + initialVelocity); + + // 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 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(); + else if (second.pitch() < pitch) + return Optional.of(second); // it's close enough + int maxIters = 50; + var range = new Pair(first, second); while (maxIters >= 0) { maxIters--; + assert range.getSecond().height() > range.getFirst().height(); - // this will throw an exception, so avoid it - // we still might have just overshot, so keep checking - if (guess < target.getZ()) - guess = target.getZ(); - - Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess); - TurretState polar = cvtShot(guessVelocity, guess); - double difference = pitch - polar.pitch(); - // System.out.println(guess + "\t\t" + guessVelocity + "\t\t" + polar.pitch() + - // "\t\t" + difference); + double guessHeight = (range.getFirst().height() + range.getSecond().height()) / 2; + TurretState guess = cvtShot(getRequiredExitVelocity(initialVelocity, target, guessHeight), guessHeight); - // we've already hit minimum height and are trying to go lower - if (guess <= target.getZ() && difference < 0) - return Optional.empty(); + // System.out.println(range + "\t\tguess=" + guess); - if (Math.abs(difference) <= tolerance) - return Optional.of(polar); + if (range.getSecond().pitch() - range.getFirst().pitch() <= tolerance) { + // we've found a valid angle + if (Math.abs(guess.pitch() - pitch) <= tolerance) + 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; + } - guess += difference * 10; + if (guess.pitch() > pitch) + range = new Pair(range.getFirst(), guess); + else + range = new Pair(guess, range.getSecond()); } - throw new RuntimeException("Solving for angle did not converge."); + throw new RuntimeException("Solving for angle did not converge (velocity: " + initialVelocity + ", target: " + + target + ", pitch: " + pitch + ", tolerance: " + tolerance + ")."); } } diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java index a303229..552e5b4 100644 --- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java +++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java @@ -174,13 +174,13 @@ class ShooterPhysicsTest { @Test public void angleTest() { - // var t1 = new Translation3d(1, 2, 3); + // var t1 = new Translation3d(10, 10, 3); // for (int i = 31; i < 1000; i++) { // var x = ShooterPhysics.getShotParams(Translation2d.kZero, t1, i / 10.); // System.out.println(i / 10. + ", " + x.pitch()); // } - var t1 = new Translation3d(100, 0, 0); + var t1 = new Translation3d(10, 0, 0); var state1 = ShooterPhysics.withAngle(Translation2d.kZero, t1, Units.degreesToRadians(30)); assertTrue(state1.isPresent()); @@ -192,7 +192,7 @@ class ShooterPhysicsTest { state1.get()); checkTrajectory(Translation3d.kZero, v1, t1, state1.get().height()); - var t2 = new Translation3d(1, -1, 100); + var t2 = new Translation3d(1, -1, 10); var state2 = ShooterPhysics.withAngle(Translation2d.kZero, t2, Units.degreesToRadians(60)); assertTrue(state2.isEmpty()); @@ -210,12 +210,20 @@ class ShooterPhysicsTest { state3.get()); checkTrajectory(Translation3d.kZero, v3.plus(new Translation3d(iv3)), t3, state3.get().height()); + + // just check this converges + var state4 = ShooterPhysics.withAngle(Translation2d.kZero, new Translation3d(10, 10, 3), Math.PI / 2 - 0.01); + assertTrue(state4.isPresent()); + assertEquals(state4.get().pitch(), Math.PI / 2 - 0.01, epsilon); + var state5 = ShooterPhysics.withAngle(Translation2d.kZero, new Translation3d(10, -10, 0), 0.01); + assertTrue(state5.isPresent()); + assertEquals(state5.get().pitch(), 0.01, epsilon); } @Test public void simpleConstraintsTest() { // a test where the optimal shot is just plain height - Constraints constraints1 = new Constraints(3, 20, .1, Math.PI - .1); + Constraints constraints1 = new Constraints(3, 20, .1, Math.PI / 2 - .1); var val1 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(1, 2, 3), constraints1); assertTrue(val1.isPresent()); var direct1 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(1, 2, 3), @@ -224,10 +232,11 @@ class ShooterPhysicsTest { assertEquals(direct1.yaw().getRadians(), val1.get().yaw().getRadians(), epsilon); assertEquals(direct1.exitVel(), val1.get().exitVel(), epsilon); - Constraints constraints2 = new Constraints(3, 20, Units.degreesToRadians(45), Math.PI - .2); - var val2 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(2, 2, 3), constraints2); + // optimal is the minimum angle + Constraints constraints2 = new Constraints(3, 30, Units.degreesToRadians(45), Math.PI / 2 - .1); + var val2 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(10, 10, 3), constraints2); assertTrue(val2.isPresent()); - assertEquals(Units.degreesToRadians(45), val2.get().pitch(), epsilon); + assertEquals(45, Units.radiansToDegrees(val2.get().pitch()), .1); } // test using a simple physics simulation