Added more tests; found more bugs.

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index 5b27e421990739df4d5a89ea91cbcfdfc2dc5162..cb8640f9ffdb95fa5d66e2931767b9b7e74abfa8 100644 (file)
--- a/src/main/java/frc/robot/util/ShooterPhysics.java
+++ b/src/main/java/frc/robot/util/ShooterPhysics.java
@@ -10,10 +10,12 @@ import frc.robot.constants.Constants;
 class ShooterPhysics {
        // pitch in radians, going up from the horizontal
        // speed in m/s
-       public record TurretState(Rotation2d yaw, double pitch, double speed){};
+       public record TurretState(Rotation2d yaw, double pitch, double speed) {
+       };
 
        @CheckReturnValue
-       public static TurretState getShotParams(Translation2d initialVelocity, Translation2d robot, Translation3d target, double height) {
+       public static TurretState getShotParams(Translation2d initialVelocity, Translation2d robot, Translation3d target,
+                       double height) {
                Translation3d robotToTarget = target.minus(new Translation3d(robot));
                Translation3d impulse = getRequiredImpulse(initialVelocity, robotToTarget, height);
 
@@ -21,7 +23,7 @@ class ShooterPhysics {
                Rotation2d yaw = as2d.getAngle();
                double magnitude2d = as2d.getDistance(Translation2d.kZero);
 
-               double pitch = Math.atan2(impulse.getZ(), magnitude2d) + Math.PI * 3 / 2;
+               double pitch = new Translation2d(magnitude2d, impulse.getZ()).getAngle().getRadians();
                pitch %= Math.PI * 2;
                double speed = impulse.getDistance(Translation3d.kZero);
 
@@ -49,12 +51,13 @@ class ShooterPhysics {
                // quadratic formula
                // t = (-v_z_impulse ± √(4 * (-.5 * g) * (-z_target))) / (2 * -.5 * g)
                // t = (-v_z_impulse ± √(2 * g * z_target)) / -g
-               // onlz use + because we only want the part where it's coming down
-               double t = (-zImpulse + Math.sqrt(2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
-                       / -Constants.GRAVITY_ACCELERATION;
+               // onlz use - because we only want the part where it's coming down, and that
+               // gives the longer time
+               double t = (-zImpulse - Math.sqrt(2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
+                               / -Constants.GRAVITY_ACCELERATION;
 
                if (t < 0)
-                       throw new RuntimeException("Time should never be negative (got: " + t + ").");
+                       throw new RuntimeException("Time should never be negative (got t=" + t + ").");
 
                // calculate x and z impulse
                // x = (v_x_robot + v_x_impulse) * t
@@ -67,4 +70,3 @@ class ShooterPhysics {
                return new Translation3d(xImpulse, yImpulse, zImpulse);
        }
 }
-

diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
index 7c6200cb88dbf828dc2a9e74a5ca71c6438d0b79..fd6631b8313fa49ee40d4b1e32a8d173c62434c1 100644 (file)
--- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java
+++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
@@ -56,7 +56,18 @@ class ShooterPhysicsTest {
        }
 
        @Test
-       public void angleTest() {
+       public void initialVelocityTest() {
+               Translation2d initialVelocityDiff = new Translation2d(5.1, -6.5);
+               Translation3d transform = ShooterPhysics.getRequiredImpulse(Translation2d.kZero, new Translation3d(1, 2, 3), 4);
+               Translation3d transform2 = ShooterPhysics.getRequiredImpulse(initialVelocityDiff, new Translation3d(1, 2, 3),
+                               4);
+               assertEquals(transform.getX(), transform2.getX() + initialVelocityDiff.getX(), epsilon);
+               assertEquals(transform.getY(), transform2.getY() + initialVelocityDiff.getY(), epsilon);
+               assertEquals(transform.getZ(), transform2.getZ(), epsilon);
+       }
+
+       @Test
+       public void yawTest() {
                ShooterPhysics.TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero, Translation2d.kZero,
                                new Translation3d(1, 0, 0), 1);
                // different for this one because it's close to 0, so the angle wraps and
@@ -76,4 +87,35 @@ class ShooterPhysicsTest {
                assertEquals(new Rotation2d(-Math.PI / 2).getRadians(), state4.yaw().getRadians(), epsilon);
        }
 
+       @Test
+       public void pitchTest() {
+               // check random values are within range
+               ShooterPhysics.TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero, Translation2d.kZero,
+                               new Translation3d(1, 0, 0), 1);
+               assertTrue(state1.pitch() >= 0 && state1.pitch() <= Math.PI / 2, state1.toString());
+
+               ShooterPhysics.TurretState state2 = ShooterPhysics.getShotParams(new Translation2d(12.2, -1.3),
+                               new Translation2d(.2, 1.2),
+                               new Translation3d(1.1, 12, 11.1), 22.1);
+               assertTrue(state2.pitch() >= 0 && state2.pitch() <= Math.PI / 2, state2.toString());
+
+               ShooterPhysics.TurretState state3 = ShooterPhysics.getShotParams(new Translation2d(1.9, 9.1),
+                               new Translation2d(-.3, -8.4),
+                               new Translation3d(11.2, -13.1, 4.1), 5.6);
+               assertTrue(state3.pitch() >= 0 && state3.pitch() <= Math.PI / 2, state3.toString());
+
+               // try a steep shot
+               ShooterPhysics.TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero, Translation2d.kZero,
+                               new Translation3d(1, 0, 99), 100);
+               assertTrue(state4.pitch() >= Math.PI * 7 / 16 && state4.pitch() <= Math.PI / 2, state4.toString());
+
+               ShooterPhysics.TurretState state5 = ShooterPhysics.getShotParams(Translation2d.kZero, Translation2d.kZero,
+                               new Translation3d(1, 0, 0), 100);
+               assertTrue(state5.pitch() >= Math.PI * 7 / 16 && state5.pitch() <= Math.PI / 2, state5.toString());
+
+               // try a shallow shot
+               ShooterPhysics.TurretState state6 = ShooterPhysics.getShotParams(Translation2d.kZero, Translation2d.kZero,
+                               new Translation3d(100, 50, 1), 2);
+               assertTrue(state6.pitch() >= 0 && state6.pitch() <= Math.PI / 16, state6.toString());
+       }
 }