Add code to calculate with a fixed velocity.

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index cb8640f9ffdb95fa5d66e2931767b9b7e74abfa8..d5b6759c84097e137db0717df75d491edb26325b 100644 (file)
--- a/src/main/java/frc/robot/util/ShooterPhysics.java
+++ b/src/main/java/frc/robot/util/ShooterPhysics.java
@@ -1,6 +1,6 @@
 package frc.robot.util;
 
-import com.google.errorprone.annotations.CheckReturnValue;
+import java.util.Optional;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -13,7 +13,6 @@ class ShooterPhysics {
        public record TurretState(Rotation2d yaw, double pitch, double speed) {
        };
 
-       @CheckReturnValue
        public static TurretState getShotParams(Translation2d initialVelocity, Translation2d robot, Translation3d target,
                        double height) {
                Translation3d robotToTarget = target.minus(new Translation3d(robot));
@@ -32,7 +31,6 @@ class ShooterPhysics {
 
        // assumes shot from (0, 0, 0)
        // only public for unit testing, don't actually use this directly
-       @CheckReturnValue
        public static Translation3d getRequiredImpulse(Translation2d initialVelocity, Translation3d target, double peakZ) {
                // z = v_z_impulse * t - .5 * g * t²
                // want vertex of this equation to equal peakZ
@@ -69,4 +67,37 @@ class ShooterPhysics {
                double yImpulse = target.getY() / t - initialVelocity.getY();
                return new Translation3d(xImpulse, yImpulse, zImpulse);
        }
+
+       // call with default tolerance
+       public static Optional<Translation3d> getImpulseForSpeed(Translation2d initialVelocity, Translation3d target,
+                       double speed) {
+               return getImpulseForSpeed(initialVelocity, target, speed, 0.1);
+       }
+
+       public static Optional<Translation3d> getImpulseForSpeed(Translation2d initialVelocity, Translation3d target,
+                       double speed, double tolerance) {
+
+               // TODO: detect when the given velocity is insufficient and exit before maxIters
+
+               // guess a peak height
+               double guess = 10;
+               int maxIters = 20;
+               while (maxIters >= 0) {
+                       maxIters--;
+                       Translation3d guessVelocity = getRequiredImpulse(initialVelocity, target, guess);
+                       double guessSpeed = guessVelocity.getNorm();
+                       double difference = speed - guessSpeed;
+
+                       // we've already hit zero height and are trying to go lower
+                       if (guess <= 0 && difference < 0)
+                               return Optional.empty();
+
+                       if (Math.abs(difference) <= tolerance)
+                               return Optional.of(guessVelocity);
+
+                       guess += difference * 1.7; // experimentally determined value
+               }
+
+               return Optional.empty();
+       }
 }

diff --git a/src/test/java/frc/robot/util/ShooterPhysicsTest.java b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
index fd6631b8313fa49ee40d4b1e32a8d173c62434c1..127d274cb79315985c23c456f9a745c400e39012 100644 (file)
--- a/src/test/java/frc/robot/util/ShooterPhysicsTest.java
+++ b/src/test/java/frc/robot/util/ShooterPhysicsTest.java
@@ -4,6 +4,8 @@ package frc.robot.util;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import java.util.Optional;
+
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
@@ -118,4 +120,18 @@ class ShooterPhysicsTest {
                                new Translation3d(100, 50, 1), 2);
                assertTrue(state6.pitch() >= 0 && state6.pitch() <= Math.PI / 16, state6.toString());
        }
+
+       @Test
+       public void velocityTest() {
+               // just make sure it converges properly
+               Optional<Translation3d> translation = ShooterPhysics.getImpulseForSpeed(new Translation2d(4.2, -3),
+                               new Translation3d(1, 2, 3), 20);
+               assertTrue(translation.isPresent());
+               assertEquals(translation.get().getNorm(), 20, 0.1);
+
+               // check something impossible is impossible
+               Optional<Translation3d> translation2 = ShooterPhysics.getImpulseForSpeed(Translation2d.kZero,
+                               new Translation3d(100, 0, 5), 10);
+               assertTrue(translation2.isEmpty());
+       }
 }