From 94c2fe4f85d4c7d1c676653fef1aa2590344acdc Mon Sep 17 00:00:00 2001 From: Arnav495 Date: Thu, 5 Feb 2026 16:32:35 -0800 Subject: [PATCH] Start adding tests. --- .../java/frc/robot/util/ShooterPhysics.java | 3 +- .../frc/robot/util/ShooterPhysicsTest.java | 48 +++++++++++++++---- 2 files changed, 39 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index 40fec43..1eb4dcf 100644 --- 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 c809896..b08981c 100644 --- 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() { -- 2.39.5