Fix withVelocity.

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index 9f6b30e8dcd9a657596951c2eae88c058dcd9178..c9bd59c9eff582c86cb7915d43c975508e95d37a 100644 (file)
--- a/src/main/java/frc/robot/util/ShooterPhysics.java
+++ b/src/main/java/frc/robot/util/ShooterPhysics.java
@@ -185,11 +185,14 @@ public class ShooterPhysics {
 
        public static TurretState withMinimumSpeed(Translation2d initialVelocity, Translation3d target,
                        double tolerance) {
+               // System.out.println("!!! inv:" + initialVelocity + " tgt:" + target + " tlr:" + tolerance);
                // trying to calculate a shot for height=0 returns NaN
-               double effectiveMinHeight = Math.max(target.getZ(), 0.01);
+               double effectiveMinHeight = Math.max(target.getZ(), 0.001);
 
                // guess a peak height
-               double guess = target.getZ() + 2;
+               double guess = effectiveMinHeight + 5;
+               double lastDerivative = Double.NaN;
+               double lastGuess = Double.NaN;
                int maxIters = 50;
                while (maxIters >= 0) {
                        maxIters--;
@@ -201,9 +204,8 @@ public class ShooterPhysics {
 
                        Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess);
                        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 <= effectiveMinHeight && derivative > 0)
@@ -212,8 +214,21 @@ public class ShooterPhysics {
                        if (Math.abs(derivative) <= tolerance)
                                return cvtShot(guessVelocity, guess);
 
-                       // desmos guesstimation indicates this works quite well
-                       guess -= derivative * (Math.pow(guess, 2) + 1) / 10;
+                       double secondDerivative;
+                       if (Double.isNaN(lastDerivative) || Double.isNaN(lastGuess))
+                               secondDerivative = 0;
+                       else
+                               secondDerivative = (lastDerivative - derivative) / (lastGuess - guess);
+
+                       assert Double.isFinite(secondDerivative);
+                       lastGuess = guess;
+                       lastDerivative = derivative;
+
+                       // System.out.println(guess + "\t\t" + "\t\t" + derivative + "\t\t" + secondDerivative);
+                       if (secondDerivative == 0)
+                               guess -= derivative * 2;
+                       else
+                               guess -= derivative / Math.abs(secondDerivative);
                }
 
                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 046004cbd3f9763227084e15195e350a9a72d949..5c3572c5a64bea2ba768a49804da59619467e13c 100644 (file)
--- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java
+++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
@@ -9,6 +9,7 @@ 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;
@@ -137,7 +138,7 @@ class ShooterPhysicsTest {
        public void velocityTest() {
 
                // var t1 = new Translation3d(100, 0, 0);
-               // for (int i = 0; i < 1000; i++) {
+               // for (int i = 1; i < 1000; i++) {
                // var x = ShooterPhysics.getShotParams(Translation2d.kZero, t1, i / 10.);
                // System.out.println(i / 10. + ", " + x.exitVel());
                // }
@@ -146,9 +147,9 @@ class ShooterPhysicsTest {
                var state1 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t1);
                // check moving either way is higher velocity
                var state1Plus = ShooterPhysics.getShotParams(Translation2d.kZero, t1,
-                               state1.height() + 0.1);
+                               state1.height() + 0.2);
                var state1Minus = ShooterPhysics.getShotParams(Translation2d.kZero, t1,
-                               state1.height() - 0.1);
+                               state1.height() - 0.2);
                assertTrue(state1.exitVel() < state1Plus.exitVel(), state1Plus.toString());
                assertTrue(state1.exitVel() < state1Minus.exitVel(), state1Minus.toString());
                assertEquals(Math.PI / 4, state1.pitch(), epsilon);
@@ -156,7 +157,8 @@ class ShooterPhysicsTest {
                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);
+               var state2Plus = ShooterPhysics.getShotParams(Translation2d.kZero, t2,
+                               state2.height() + 0.1);
                assertTrue(state2.exitVel() < state2Plus.exitVel(), state2Plus.toString());
                assertEquals(t2.getZ(), state2.height(), epsilon);
 
@@ -165,8 +167,8 @@ class ShooterPhysicsTest {
                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);
+               var state3Plus = ShooterPhysics.getShotParams(v3, t3, state3.height() + 0.2);
+               var state3Minus = ShooterPhysics.getShotParams(v3, t3, state3.height() - 0.2);
                assertTrue(state3.exitVel() < state3Plus.exitVel(), state3Plus.toString());
                assertTrue(state3.exitVel() < state3Minus.exitVel(), state3Minus.toString());
        }
@@ -255,12 +257,12 @@ class ShooterPhysicsTest {
 
                // compute 1000 random shots and simulate them
                for (int i = 0; i < 1000; i++) {
-                       Translation2d initPos = new Translation2d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10);
-                       Translation3d target = new Translation3d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10,
-                                       rng.nextDouble() * 10 + 1);
-                       Translation2d initVel = new Translation2d(rng.nextDouble() * 10 - 5, rng.nextDouble() * 10 - 5);
+                       Translation2d initPos = new Translation2d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10));
+                       Translation3d target = new Translation3d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10),
+                                       rng.nextDouble(1, 11));
+                       Translation2d initVel = new Translation2d(rng.nextDouble(-5, 5), rng.nextDouble(-5, 5));
                        Translation3d robotToTarget = target.minus(new Translation3d(initPos));
-                       double arcHeight = target.getZ() + rng.nextDouble() * 10;
+                       double arcHeight = target.getZ() + rng.nextDouble(10);
 
                        Translation3d exitVel = ShooterPhysics.getRequiredExitVelocity(initVel, robotToTarget, arcHeight);
 
@@ -269,6 +271,37 @@ class ShooterPhysicsTest {
                }
        }
 
+       @Disabled
+       @Test
+       public void simulatedConstraintsTest() {
+               Random rng = new Random(6328);
+
+               for (int i = 0; i < 1000; i++) {
+                       Translation2d initPos = new Translation2d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10));
+                       Translation3d target = new Translation3d(rng.nextDouble(-10, 10), rng.nextDouble(-10, 10),
+                                       rng.nextDouble(1, 11));
+                       Translation2d initVel = new Translation2d(rng.nextDouble(-5, 5), rng.nextDouble(-5, 5));
+                       Translation3d robotToTarget = target.minus(new Translation3d(initPos));
+
+                       double minAngle = rng.nextDouble(.01, Math.PI / 2 - .03);
+                       Constraints constraints = new Constraints(rng.nextDouble(20), rng.nextDouble(30), minAngle,
+                                       rng.nextDouble(minAngle + .01, Math.PI / 2 - .01));
+
+                       var state = ShooterPhysics.getConstrainedParams(initVel, robotToTarget, constraints);
+
+                       if (state.isPresent()) {
+                               assertTrue(state.get().satisfies(constraints));
+
+                               // check going down breaks a constraint; we've found the minimum
+                               var lower = ShooterPhysics.withAngle(initVel, robotToTarget, state.get().pitch() - .1);
+                               assertTrue(lower.isEmpty());
+                               // check going up is okay
+                               var higher = ShooterPhysics.withAngle(initVel, robotToTarget, state.get().pitch() + .1);
+                               assertTrue(higher.isPresent());
+                       }
+               }
+       }
+
        private class PhysicsObject {
                private Translation3d pos;
                private Translation3d vel;