From 088e4fecd7cabb06d7cfa96f3c9535d8eacadfc6 Mon Sep 17 00:00:00 2001
From: Arnav495 <arnieincyberland@gmail.com>
Date: Thu, 5 Feb 2026 10:42:14 -0800
Subject: [PATCH] Copy changes from shooter-fall-project.

---
 .../java/frc/robot/util/ShooterPhysics.java   | 42 ++++++++++++++++---
 1 file changed, 37 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
index 3cf51d1..d66eadc 100644
--- a/src/main/java/frc/robot/util/ShooterPhysics.java
+++ b/src/main/java/frc/robot/util/ShooterPhysics.java
@@ -17,14 +17,13 @@ public class ShooterPhysics {
 	 * Calculates shot parameters for SOTM using Physics™.
 	 *
 	 * @param robotVelocity The x and y velocity of the robot, field relative.
-	 * @param robot         The position of the center of the robot, field relative.
-	 * @param target        The position of the target, field relative.
+	 * @param robotToTarget The robot to target transform. Angles are field
+	 *                      relative, positions are with the robot as the origin.
 	 * @param height        The peak height the trajectory should reach.
 	 * @return A TurretState that represents the shot the robot should take.
 	 */
-	public static TurretState getShotParams(Translation2d robotVelocity, Translation2d robot, Translation3d target,
+	public static TurretState getShotParams(Translation2d robotVelocity, Translation3d robotToTarget,
 			double height) {
-		Translation3d robotToTarget = target.minus(new Translation3d(robot));
 		Translation3d exitVel = getRequiredExitVelocity(robotVelocity, robotToTarget, height);
 
 		Translation2d onGround = exitVel.toTranslation2d();
@@ -38,6 +37,27 @@ public class ShooterPhysics {
 		return new TurretState(yaw, pitch, speed);
 	}
 
+	public static Optional<TurretState> getShotParamsBySpeed(Translation2d robotVelocity, Translation2d robot,
+			Translation3d target,
+			double speed) {
+		Translation3d robotToTarget = target.minus(new Translation3d(robot));
+		Optional<Translation3d> exitVelOpt = getExitVelocityForSpeed(robotVelocity, robotToTarget, speed);
+
+		if (exitVelOpt.isEmpty())
+			return Optional.empty();
+		Translation3d exitVel = exitVelOpt.get();
+
+		Translation2d onGround = exitVel.toTranslation2d();
+		Rotation2d yaw = onGround.getAngle();
+		double magnitude2d = onGround.getNorm();
+
+		double pitch = new Translation2d(magnitude2d, exitVel.getZ()).getAngle().getRadians();
+		pitch %= Math.PI * 2;
+		double speed2 = exitVel.getDistance(Translation3d.kZero);
+
+		return Optional.of(new TurretState(yaw, pitch, speed2));
+	}
+
 	/**
 	 * Actually does the SOTM math. Assumes shots are from (0, 0, 0). This is only
 	 * public so it can be unit tested, and shouldn't be called directly.
@@ -102,7 +122,19 @@ public class ShooterPhysics {
 	public static Optional<Translation3d> getExitVelocityForSpeed(Translation2d initialVelocity, Translation3d target,
 			double speed, double tolerance) {
 
-		// TODO: detect when the given velocity is insufficient and exit before maxIters
+		// calculate minimum velocity: v² = g * (R + √(R² + h²))
+		double horizontalDist = target.toTranslation2d().getNorm();
+		double verticalDist = target.getZ();
+		double g = Constants.GRAVITY_ACCELERATION;
+		double robotSpeed = initialVelocity.getNorm();
+
+		double minProjectileSpeed = Math
+				.sqrt(g * (horizontalDist + Math.sqrt(horizontalDist * horizontalDist + verticalDist * verticalDist)));
+		double minSpeed = Math.max(0, minProjectileSpeed - robotSpeed);
+
+		if (speed < minSpeed - tolerance) {
+			return Optional.empty();
+		}
 
 		// guess a peak height
 		double guess = target.getZ() + 2;
-- 
2.39.5