Add test for velocity.

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index f6b7c3239641d3ea2acb13ab1117da41ce1c6f0e..975f4c2dc082e3bd7d14c8a9ce91d631a20b399b 100644 (file)
--- 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 b08981c392dbc266f4ef58b0b01ce2f432d9d47f..37502162f636a88a613f26c9682c706220bc5993 100644 (file)
--- 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