Add test for .withAngle()

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index 975f4c2dc082e3bd7d14c8a9ce91d631a20b399b..64f21485a62a59430694948ba70c80a815d0f30a 100644 (file)
--- 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<TurretState> withAngle(Translation2d initialVelocity, Translation3d target,
                        double pitch) {
-               return withAngle(initialVelocity, target, pitch, Units.degreesToRadians(1));
+               return withAngle(initialVelocity, target, pitch, .01);
        }
 
        public static Optional<TurretState> 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 37502162f636a88a613f26c9682c706220bc5993..7add5dd018d6aef0714482984daf31c1c71e6718 100644 (file)
--- 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)