From: Arnav495 <arnieincyberland@gmail.com>
Date: Thu, 15 Jan 2026 04:53:56 +0000 (-0800)
Subject: Add code to calculate with a fixed velocity.
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=9dd767ede15b49825c5693a393bbe8ac3fca7a0d;p=FRC2026.git

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 cb8640f..d5b6759 100644
--- 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 fd6631b..127d274 100644
--- 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());
+	}
 }