From baf6c1d3dcf72188777dc7d7122ce00f7a09fc57 Mon Sep 17 00:00:00 2001 From: Arnav495 Date: Thu, 15 Jan 2026 15:13:40 -0800 Subject: [PATCH] Add more tests for shooter physics; find more bugs. I really can't do the quadratic formula correctly, can I. --- .../java/frc/robot/util/ShooterPhysics.java | 10 +- .../frc/robot/util/ShooterPhysicsTest.java | 96 ++++++++++++++++++- 2 files changed, 99 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index 0aa65b8..061cb4a 100644 --- 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 ed0bee1..50076ce 100644 --- 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(); + 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 + ")."); + } } -- 2.39.5