From 91b958491c9e5372f87fd5dbd18b60f29dd314cd Mon Sep 17 00:00:00 2001 From: Arnav495 Date: Thu, 5 Feb 2026 18:42:04 -0800 Subject: [PATCH] Add test for velocity. --- .../java/frc/robot/util/ShooterPhysics.java | 33 ++++++++--------- .../frc/robot/util/ShooterPhysicsTest.java | 36 ++++++++++++++++--- 2 files changed, 45 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index f6b7c32..975f4c2 100644 --- a/src/main/java/frc/robot/util/ShooterPhysics.java +++ b/src/main/java/frc/robot/util/ShooterPhysics.java @@ -174,44 +174,39 @@ public class ShooterPhysics { // call with default tolerance public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target) { - return withMinimumSpeed(initialVelocity, target, 0.1); + return withMinimumSpeed(initialVelocity, target, 0.001); } public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target, double tolerance) { - - // calculate minimum velocity: v² = g * (R + √(R² + h²)) - double horizontalDist = target.toTranslation2d().getNorm(); - double verticalDist = target.getZ(); - double g = Constants.GRAVITY_ACCELERATION; - double robotSpeed = initialVelocity.getNorm(); - - double minProjectileSpeed = Math.sqrt(g * (horizontalDist + Math.hypot(horizontalDist, verticalDist))); - double minSpeed = Math.max(0, minProjectileSpeed - robotSpeed); + // trying to calculate a shot for height=0 returns NaN + double effectiveMinHeight = Math.max(target.getZ(), 0.01); // guess a peak height double guess = target.getZ() + 2; - int maxIters = 20; + int maxIters = 50; while (maxIters >= 0) { maxIters--; // this will throw an exception, so avoid it // we still might have just overshot, so keep checking - if (guess < target.getZ()) - guess = target.getZ(); + if (guess < effectiveMinHeight) + guess = effectiveMinHeight; Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess); - double guessSpeed = guessVelocity.getNorm(); - double difference = minSpeed - guessSpeed; + Translation3d guessVelocityMore = getRequiredExitVelocity(initialVelocity, target, guess + 0.1); + double derivative = (guessVelocityMore.getNorm() - guessVelocity.getNorm()) / 0.1; + // System.out.println(guess + "\t\t" + guessVelocity.getNorm() + "\t\t" + derivative); // we've already hit minimum height and are trying to go lower - if (guess <= target.getZ() && difference < 0) - throw new RuntimeException("Incorrect minimum speed calculation in ShooterPhysics.java"); + if (guess <= effectiveMinHeight && derivative > 0) + return cvtShot(guessVelocity, guess); - if (Math.abs(difference) <= tolerance) + if (Math.abs(derivative) <= tolerance) return cvtShot(guessVelocity, guess); - guess += difference * 1.7; // experimentally determined value + // desmos guesstimation indicates this works quite well + guess -= derivative * (Math.pow(guess, 2) + 1) / 10; } throw new RuntimeException("Failed to compute a trajectory for a minimum speed."); diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java index b08981c..3750216 100644 --- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java +++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java @@ -134,13 +134,39 @@ class ShooterPhysicsTest { @Test public void velocityTest() { - var t1 = new Translation3d(1, 0, 2); + + // var t1 = new Translation3d(100, 0, 0); + // for (int i = 0; i < 1000; i++) { + // var x = ShooterPhysics.getShotParams(Translation2d.kZero, t1, i / 10.); + // System.out.println(i / 10. + ", " + x.exitVel()); + // } + + var t1 = new Translation3d(100, 0, 0); var state1 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t1); // check moving either way is higher velocity - assertTrue(state1.exitVel() < ShooterPhysics.getShotParams(Translation2d.kZero, t1, state1.height() + 0.1) - .exitVel()); - assertTrue(state1.exitVel() < ShooterPhysics.getShotParams(Translation2d.kZero, t1, state1.height() - 0.1) - .exitVel()); + var state1Plus = ShooterPhysics.getShotParams(Translation2d.kZero, t1, + state1.height() + 0.1); + var state1Minus = ShooterPhysics.getShotParams(Translation2d.kZero, t1, + state1.height() - 0.1); + assertTrue(state1.exitVel() < state1Plus.exitVel(), state1Plus.toString()); + assertTrue(state1.exitVel() < state1Minus.exitVel(), state1Minus.toString()); + + var t2 = new Translation3d(1, 1, 100); + var state2 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t2); + // this should get to the minimum height + var state2Plus = ShooterPhysics.getShotParams(Translation2d.kZero, t2, state2.height() + 0.1); + assertTrue(state2.exitVel() < state2Plus.exitVel(), state2Plus.toString()); + assertEquals(t2.getZ(), state2.height(), epsilon); + + // test with an initial velocity + var t3 = new Translation3d(100, 0, 0); + var v3 = new Translation2d(10, -20); + var state3 = ShooterPhysics.withMinimumSpeed(v3, t3); + // check moving either way is higher velocity + var state3Plus = ShooterPhysics.getShotParams(v3, t3, state3.height() + 0.1); + var state3Minus = ShooterPhysics.getShotParams(v3, t3, state3.height() - 0.1); + assertTrue(state3.exitVel() < state3Plus.exitVel(), state3Plus.toString()); + assertTrue(state3.exitVel() < state3Minus.exitVel(), state3Minus.toString()); } @Test -- 2.39.5