From 76c1b99675f030034ab822146d3de7a12ccc98d3 Mon Sep 17 00:00:00 2001 From: Arnav495 Date: Mon, 12 Jan 2026 11:35:07 -0800 Subject: [PATCH] Added more tests; found more bugs. --- .../java/frc/robot/util/ShooterPhysics.java | 18 ++++---- .../frc/robot/util/ShooterPhysicsTest.java | 44 ++++++++++++++++++- 2 files changed, 53 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index 5b27e42..cb8640f 100644 --- 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 7c6200c..fd6631b 100644 --- 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()); + } } -- 2.39.5