small fixes

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index f6b7c3239641d3ea2acb13ab1117da41ce1c6f0e..f6381c3daa681c3ab8ebebf5eb08f68c8108ef21 100644 (file)
--- a/src/main/java/frc/robot/util/ShooterPhysics.java
+++ b/src/main/java/frc/robot/util/ShooterPhysics.java
@@ -111,7 +111,6 @@ public class ShooterPhysics {
                double magnitude2d = onGround.getNorm();
 
                double pitch = new Translation2d(magnitude2d, velocity.getZ()).getAngle().getRadians();
-               pitch %= Math.PI * 2;
                double speed = velocity.getDistance(Translation3d.kZero);
 
                return new TurretState(yaw, pitch, speed, height);
@@ -204,9 +203,9 @@ public class ShooterPhysics {
                        double guessSpeed = guessVelocity.getNorm();
                        double difference = minSpeed - guessSpeed;
 
-                       // 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 we're at minimum height and still need more speed, it's impossible
+                       if (guess <= target.getZ() && difference > 0)
+                               throw new RuntimeException("Impossible to achieve minimum speed for target " + target);
 
                        if (Math.abs(difference) <= tolerance)
                                return cvtShot(guessVelocity, guess);

diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
index b08981c392dbc266f4ef58b0b01ce2f432d9d47f..d0342780b662850f53bf088d6bd80f888a9af4ae 100644 (file)
--- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java
+++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
@@ -143,11 +143,6 @@ class ShooterPhysicsTest {
                                .exitVel());
        }
 
-       @Test
-       public void angleTest() {
-
-       }
-
        @Test
        public void simpleConstraintsTest() {
                Constraints constraints = new Constraints(3, 20, .1, Math.PI - .1);