From 4e51827c0bc9339d9cc42806a8cdb9a1388cf5d6 Mon Sep 17 00:00:00 2001 From: Arnav495 Date: Thu, 5 Feb 2026 20:14:08 -0800 Subject: [PATCH] Add test for .withAngle() --- .../java/frc/robot/util/ShooterPhysics.java | 8 ++-- .../frc/robot/util/ShooterPhysicsTest.java | 39 ++++++++++++++++++- 2 files changed, 41 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java index 975f4c2..64f2148 100644 --- a/src/main/java/frc/robot/util/ShooterPhysics.java +++ b/src/main/java/frc/robot/util/ShooterPhysics.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; import frc.robot.constants.Constants; public class ShooterPhysics { @@ -214,7 +213,7 @@ public class ShooterPhysics { public static Optional withAngle(Translation2d initialVelocity, Translation3d target, double pitch) { - return withAngle(initialVelocity, target, pitch, Units.degreesToRadians(1)); + return withAngle(initialVelocity, target, pitch, .01); } public static Optional withAngle(Translation2d initialVelocity, Translation3d target, @@ -222,7 +221,7 @@ public class ShooterPhysics { // guess a peak height double guess = target.getZ() + 2; - int maxIters = 20; + int maxIters = 50; while (maxIters >= 0) { maxIters--; @@ -234,6 +233,7 @@ public class ShooterPhysics { Translation3d guessVelocity = getRequiredExitVelocity(initialVelocity, target, guess); TurretState polar = cvtShot(guessVelocity, guess); double difference = pitch - polar.pitch(); + // System.out.println(guess + "\t\t" + difference); // we've already hit minimum height and are trying to go lower if (guess <= target.getZ() && difference < 0) @@ -242,7 +242,7 @@ public class ShooterPhysics { if (Math.abs(difference) <= tolerance) return Optional.of(polar); - guess += difference * 0.7; // TODO: find better value + guess += difference * 10; } throw new RuntimeException("Solving for angle did not converge."); diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java index 3750216..7add5dd 100644 --- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java +++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java @@ -9,17 +9,19 @@ import java.util.Random; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Disabled; 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 edu.wpi.first.math.util.Units; 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; + private static final double epsilon = .01; @BeforeEach public void prepare() { @@ -150,6 +152,7 @@ class ShooterPhysicsTest { state1.height() - 0.1); assertTrue(state1.exitVel() < state1Plus.exitVel(), state1Plus.toString()); assertTrue(state1.exitVel() < state1Minus.exitVel(), state1Minus.toString()); + assertEquals(Math.PI/4, state1.pitch(), epsilon); var t2 = new Translation3d(1, 1, 100); var state2 = ShooterPhysics.withMinimumSpeed(Translation2d.kZero, t2); @@ -172,8 +175,40 @@ class ShooterPhysicsTest { @Test public void angleTest() { + // var t1 = new Translation3d(100, 0, 0); + // for (int i = 0; i < 1000; i++) { + // var x = ShooterPhysics.getShotParams(Translation2d.kZero, t1, i / 10.); + // System.out.println(i / 10. + ", " + x.pitch()); + // } + + var t1 = new Translation3d(100, 0, 0); + var state1 = ShooterPhysics.withAngle(Translation2d.kZero, t1, + Units.degreesToRadians(30)); + assertTrue(state1.isPresent()); + assertEquals(state1.get().pitch(), Units.degreesToRadians(30), epsilon); + // get this as a velocity vector + Translation3d v1 = ShooterPhysics.getRequiredExitVelocity(Translation2d.kZero, t1, state1.get().height()); + assertEquals(ShooterPhysics.cvtShot(v1, state1.get().height()), state1.get()); + checkTrajectory(Translation3d.kZero, v1, t1, state1.get().height()); + + var t2 = new Translation3d(1, -1, 100); + var state2 = ShooterPhysics.withAngle(Translation2d.kZero, t2, + Units.degreesToRadians(60)); + assertTrue(state2.isEmpty()); + + var t3 = new Translation3d(1.34, 4.2, 2.3); + var iv3 = new Translation2d(12.1, -2.1); + var state3 = ShooterPhysics.withAngle(iv3, t3, + Units.degreesToRadians(45)); + assertTrue(state3.isPresent()); + assertEquals(state3.get().pitch(), Units.degreesToRadians(45), epsilon); + // get this as a velocity vector + Translation3d v3 = ShooterPhysics.getRequiredExitVelocity(iv3, t3, state3.get().height()); + assertEquals(ShooterPhysics.cvtShot(v3, state3.get().height()), state3.get()); + checkTrajectory(Translation3d.kZero, v3.plus(new Translation3d(iv3)), t3, state3.get().height()); } + @Disabled @Test public void simpleConstraintsTest() { Constraints constraints = new Constraints(3, 20, .1, Math.PI - .1); @@ -238,7 +273,7 @@ class ShooterPhysicsTest { + " and peak height " + requiredHeight + "."); messages.add("position, velocity"); // column headers - while (object.pos.getZ() > 0 || firstLoop) { + while (object.pos.getZ() > -epsilon || firstLoop) { messages.add("" + object.pos + ", " + object.vel); if (object.pos.getZ() + tolerance >= requiredHeight) -- 2.39.5