Start adding tests.

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index 40fec43d30448fe3cc1ef3b3fcf0d87088ffb0fd..1eb4dcfb45db50eb141af44242ec15fd28fe17cb 100644 (file)
--- a/src/main/java/frc/robot/util/ShooterPhysics.java
+++ b/src/main/java/frc/robot/util/ShooterPhysics.java
@@ -186,8 +186,7 @@ public class ShooterPhysics {
                double g = Constants.GRAVITY_ACCELERATION;
                double robotSpeed = initialVelocity.getNorm();
 
-               double minProjectileSpeed = Math
-                               .sqrt(g * (horizontalDist + Math.sqrt(horizontalDist * horizontalDist + verticalDist * verticalDist)));
+               double minProjectileSpeed = Math.sqrt(g * (horizontalDist + Math.hypot(horizontalDist, verticalDist)));
                double minSpeed = Math.max(0, minProjectileSpeed - robotSpeed);
 
                // guess a peak height

diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
index c809896cef9acc8fa0b68e352f45ca4fe04c66a2..b08981c392dbc266f4ef58b0b01ce2f432d9d47f 100644 (file)
--- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java
+++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
@@ -15,6 +15,8 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import frc.robot.constants.Constants;
+import frc.robot.util.ShooterPhysics.Constraints;
+import frc.robot.util.ShooterPhysics.TurretState;
 
 class ShooterPhysicsTest {
        private static final double epsilon = .001;
@@ -81,21 +83,21 @@ class ShooterPhysicsTest {
 
        @Test
        public void yawTest() {
-               ShooterPhysics.TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero,
+               TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero,
                                new Translation3d(1, 0, 0), 1);
                // different for this one because it's close to 0, so the angle wraps and
                // assertEquals can't handle that
                assertTrue(Math.abs((state1.yaw()).getRadians()) <= epsilon, state1.toString());
 
-               ShooterPhysics.TurretState state2 = ShooterPhysics.getShotParams(Translation2d.kZero,
+               TurretState state2 = ShooterPhysics.getShotParams(Translation2d.kZero,
                                new Translation3d(0, 1, 0), 1);
                assertEquals(new Rotation2d(Math.PI / 2).getRadians(), state2.yaw().getRadians(), epsilon);
 
-               ShooterPhysics.TurretState state3 = ShooterPhysics.getShotParams(Translation2d.kZero,
+               TurretState state3 = ShooterPhysics.getShotParams(Translation2d.kZero,
                                new Translation3d(-1, 0, 0), 1);
                assertEquals(new Rotation2d(Math.PI).getRadians(), state3.yaw().getRadians(), epsilon);
 
-               ShooterPhysics.TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero,
+               TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero,
                                new Translation3d(0, -1, 0), 1);
                assertEquals(new Rotation2d(-Math.PI / 2).getRadians(), state4.yaw().getRadians(), epsilon);
        }
@@ -103,33 +105,59 @@ class ShooterPhysicsTest {
        @Test
        public void pitchTest() {
                // check random values are within range
-               ShooterPhysics.TurretState state1 = ShooterPhysics.getShotParams(Translation2d.kZero,
+               TurretState state1 = ShooterPhysics.getShotParams(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),
+               TurretState state2 = ShooterPhysics.getShotParams(new Translation2d(12.2, -1.3),
                                new Translation3d(1.1, 12, 11.1).minus(new Translation3d(.2, 1.2, 0)), 22.1);
                assertTrue(state2.pitch() >= 0 && state2.pitch() <= Math.PI / 2, state2.toString());
 
-               ShooterPhysics.TurretState state3 = ShooterPhysics.getShotParams(new Translation2d(1.9, 9.1),
+               TurretState state3 = ShooterPhysics.getShotParams(new Translation2d(1.9, 9.1),
                                new Translation3d(11.2, -13.1, 4.1).minus(new Translation3d(-.3, -8.4, 0)), 5.6);
                assertTrue(state3.pitch() >= 0 && state3.pitch() <= Math.PI / 2, state3.toString());
 
                // try a steep shot
-               ShooterPhysics.TurretState state4 = ShooterPhysics.getShotParams(Translation2d.kZero,
+               TurretState state4 = ShooterPhysics.getShotParams(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,
+               TurretState state5 = ShooterPhysics.getShotParams(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,
+               TurretState state6 = ShooterPhysics.getShotParams(Translation2d.kZero,
                                new Translation3d(100, 50, 1), 2);
                assertTrue(state6.pitch() >= 0 && state6.pitch() <= Math.PI / 16, state6.toString());
        }
 
+       @Test
+       public void velocityTest() {
+               var t1 = new Translation3d(1, 0, 2);
+               var state1 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t1);
+               // check moving either way is higher velocity
+               assertTrue(state1.exitVel() < ShooterPhysics.getShotParams(Translation2d.kZero, t1, state1.height() + 0.1)
+                               .exitVel());
+               assertTrue(state1.exitVel() < ShooterPhysics.getShotParams(Translation2d.kZero, t1, state1.height() - 0.1)
+                               .exitVel());
+       }
+
+       @Test
+       public void angleTest() {
+
+       }
+
+       @Test
+       public void simpleConstraintsTest() {
+               Constraints constraints = new Constraints(3, 20, .1, Math.PI - .1);
+               var val1 = ShooterPhysics.getConstrainedParams(Translation2d.kZero, new Translation3d(1, 2, 3), constraints);
+               assertTrue(val1.isPresent());
+               var direct1 = ShooterPhysics.getShotParams(Translation2d.kZero, new Translation3d(1, 2, 3),
+                               constraints.height());
+               assertEquals(direct1, val1);
+       }
+
        // test using a simple physics simulation
        @Test
        public void simulatedTest() {