Add more tests for shooter physics; find more bugs.

I really can't do the quadratic formula correctly, can I.

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index 0aa65b82b2b59962f29e195eb3d7804a290a887e..061cb4ad4696c538cee72b617a5c720b55f071b8 100644 (file)
--- a/src/main/java/frc/robot/util/ShooterPhysics.java
+++ b/src/main/java/frc/robot/util/ShooterPhysics.java
@@ -31,7 +31,8 @@ class ShooterPhysics {
 
        // assumes shot from (0, 0, 0)
        // only public for unit testing, don't actually use this directly
-       public static Translation3d getRequiredExitVelocity(Translation2d robotVelocity, Translation3d target, double peakZ) {
+       public static Translation3d getRequiredExitVelocity(Translation2d robotVelocity, Translation3d target,
+                       double peakZ) {
                // z = v_z_exit_vel * t - .5 * g * t²
                // want vertex of this equation to equal peakZ
                // t_vertex = -v_z_exit_vel / -g
@@ -47,11 +48,12 @@ class ShooterPhysics {
                // z_target = v_z_exit_vel * t - .5 * g * t²
                // 0 = -.5 * g * t² + v_z_exit_vel * t - z_target
                // quadratic formula
-               // t = (-v_z_exit_vel ± √(4 * (-.5 * g) * (-z_target))) / (2 * -.5 * g)
-               // t = (-v_z_exit_vel ± √(2 * g * z_target)) / -g
+               // t = (-v_z_exit_vel ± √(v_z_exit_vel² - 4 * (-.5 * g) * (-z_target))) / (2 *
+               // -.5 * g)
+               // t = (-v_z_exit_vel ± √(v_z_exit_vel² - 2 * g * z_target)) / -g
                // onlz use - because we only want the part where it's coming down, and that
                // gives the longer time
-               double t = (-zExitVel - Math.sqrt(2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
+               double t = (-zExitVel - Math.sqrt(Math.pow(zExitVel, 2) - 2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
                                / -Constants.GRAVITY_ACCELERATION;
 
                if (t < 0)

diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
index ed0bee13aca442fa6cef5072dd8327bb5afcd165..50076cea3513769d538adfe814bdd0d752b1155d 100644 (file)
--- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java
+++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
@@ -4,7 +4,9 @@ package frc.robot.util;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import java.util.ArrayList;
 import java.util.Optional;
+import java.util.Random;
 
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
@@ -13,6 +15,7 @@ import org.junit.jupiter.api.Test;
 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;
 
 class ShooterPhysicsTest {
        private static final double epsilon = .001;
@@ -32,7 +35,8 @@ class ShooterPhysicsTest {
                assertEquals(0, transform.getY(), epsilon);
                assertEquals(4.427, transform.getZ(), epsilon);
 
-               Translation3d transform2 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, new Translation3d(10, 0, 0),
+               Translation3d transform2 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero,
+                               new Translation3d(10, 0, 0),
                                1);
                assertTrue(transform2.getX() > 0, transform2.toString());
                assertEquals(0, transform2.getY(), epsilon);
@@ -55,13 +59,21 @@ class ShooterPhysicsTest {
                Translation3d transform6 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero,
                                new Translation3d(-16.32, 3.3, 0), 2);
                assertEquals(-16.32 / 3.3, transform6.getX() / transform6.getY(), epsilon);
+
+               Translation3d transform7 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero,
+                               new Translation3d(2, 0, 0), Constants.GRAVITY_ACCELERATION / 2);
+               assertEquals(1, transform7.getX(), epsilon, transform7.toString());
+               assertEquals(0, transform7.getY(), epsilon);
+               assertEquals(Constants.GRAVITY_ACCELERATION, transform7.getZ(), epsilon);
        }
 
        @Test
        public void initialVelocityTest() {
                Translation2d initialVelocityDiff = new Translation2d(5.1, -6.5);
-               Translation3d transform = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, new Translation3d(1, 2, 3), 4);
-               Translation3d transform2 = ShooterPhysics.getRequiredExitVelocity(initialVelocityDiff, new Translation3d(1, 2, 3),
+               Translation3d transform = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero,
+                               new Translation3d(1, 2, 3), 4);
+               Translation3d transform2 = ShooterPhysics.getRequiredExitVelocity(initialVelocityDiff,
+                               new Translation3d(1, 2, 3),
                                4);
                assertEquals(transform.getX(), transform2.getX() + initialVelocityDiff.getX(), epsilon);
                assertEquals(transform.getY(), transform2.getY() + initialVelocityDiff.getY(), epsilon);
@@ -134,4 +146,82 @@ class ShooterPhysicsTest {
                                new Translation3d(100, 0, 5), 10);
                assertTrue(translation2.isEmpty());
        }
+
+       // test using a simple physics simulation
+       @Test
+       public void simulatedTest() {
+               // test the simulation itself
+               checkTrajectory(Translation3d.kZero, new Translation3d(1, 0, Constants.GRAVITY_ACCELERATION),
+                               new Translation3d(2, 0, .1), 2);
+
+               Random rng = new Random(972);
+
+               // compute 1000 random shots and simulate them
+               for (int i = 0; i < 1000; i++) {
+                       Translation2d initPos = new Translation2d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10);
+                       Translation3d target = new Translation3d(rng.nextDouble() * 20 - 10, rng.nextDouble() * 20 - 10,
+                                       rng.nextDouble() * 10 + 1);
+                       Translation2d initVel = new Translation2d(rng.nextDouble() * 10 - 5, rng.nextDouble() * 10 - 5);
+                       Translation3d robotToTarget = target.minus(new Translation3d(initPos));
+                       double arcHeight = target.getZ() + rng.nextDouble() * 10;
+
+                       Translation3d exitVel = ShooterPhysics.getRequiredExitVelocity(initVel, robotToTarget, arcHeight);
+
+                       checkTrajectory(new Translation3d(initPos), exitVel.plus(new Translation3d(initVel)), target,
+                                       arcHeight);
+               }
+       }
+
+       private class PhysicsObject {
+               private Translation3d pos;
+               private Translation3d vel;
+
+               private PhysicsObject(Translation3d pos, Translation3d vel) {
+                       this.pos = pos;
+                       this.vel = vel;
+               }
+
+               private void step(double time) {
+                       pos = pos.plus(vel.times(time));
+                       vel = vel.plus(new Translation3d(0, 0, -Constants.GRAVITY_ACCELERATION).times(time));
+               }
+       }
+
+       // throws an error if something goes wrong
+       private void checkTrajectory(Translation3d initPos, Translation3d initVel, Translation3d endPos,
+                       double requiredHeight) {
+               final double tolerance = .2;
+               PhysicsObject object = new PhysicsObject(initPos, initVel);
+               boolean achievedTargetHeight = false;
+               boolean firstLoop = true; // to allow starting from ground
+
+               // for diagnostics if something fails
+               var messages = new ArrayList<String>();
+               messages.add("Starting trajectory check from " + initPos + " to " + endPos + " with velocity " + initVel
+                               + " and peak height " + requiredHeight + ".");
+               messages.add("position, velocity"); // column headers
+
+               while (object.pos.getZ() > 0 || firstLoop) {
+                       messages.add("" + object.pos + ", " + object.vel);
+
+                       if (object.pos.getZ() + tolerance >= requiredHeight)
+                               achievedTargetHeight = true;
+
+                       // hit the target, check we got the needed height as well
+                       if (object.pos.getDistance(endPos) <= tolerance && achievedTargetHeight)
+                               return;
+
+                       object.step(.01);
+                       firstLoop = false;
+               }
+
+               for (String i : messages)
+                       System.out.println(i);
+
+               if (achievedTargetHeight)
+                       throw new RuntimeException("Trajectory did not hit the target (" + endPos + ").");
+               else
+                       throw new RuntimeException("Trajectory did not hit the target (" + endPos
+                                       + ") and did not attain required arc height (" + requiredHeight + ").");
+       }
 }