It passes!

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index 2589fb4f4525452efc7358f8431470b8a9e8e31f..f207dac4e798f40d0886b8b8e9434447c4968678 100644 (file)
--- 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 5c3572c5a64bea2ba768a49804da59619467e13c..f63d59814c1a269bfc4ee253facf358c543896d4 100644 (file)
--- 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());
+                               }
                        }
                }
        }