More bugfixes.

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index dad2995f2ad0fc6af2516dc9b94882dbf9917f12..9f6b30e8dcd9a657596951c2eae88c058dcd9178 100644 (file)
--- a/src/main/java/frc/robot/util/ShooterPhysics.java
+++ b/src/main/java/frc/robot/util/ShooterPhysics.java
@@ -66,6 +66,8 @@ public class ShooterPhysics {
                // now we need to satisfy the speed constraint
 
                TurretState withMinSpeed = withMinimumSpeed(robotVelocity, robotToTarget);
+               if (withMinSpeed.exitVel() > constraints.maxVel())
+                       return Optional.empty();
                // ordered such that the first element is valid and the second is not
                Pair<TurretState, TurretState> newRange;
 

diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
index 552e5b40c5b230ad2c008f067cda75341d39cdb5..046004cbd3f9763227084e15195e350a9a72d949 100644 (file)
--- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java
+++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
@@ -237,6 +237,11 @@ class ShooterPhysicsTest {
                var val2 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(10, 10, 3), constraints2);
                assertTrue(val2.isPresent());
                assertEquals(45, Units.radiansToDegrees(val2.get().pitch()), .1);
+
+               // something impossible
+               Constraints constraints3 = new Constraints(3, 3, Units.degreesToRadians(45), Math.PI / 2 - .1);
+               var val3 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(10, 10, 3), constraints3);
+               assertTrue(val3.isEmpty(), val3.toString());
        }
 
        // test using a simple physics simulation