From 4c27bea5d7b261b341de93eb5766fb2d29e442d4 Mon Sep 17 00:00:00 2001
From: Arnav495 <arnieincyberland@gmail.com>
Date: Sat, 10 Jan 2026 21:16:57 -0800
Subject: [PATCH] Add code for shooter physics.

---
 .../java/frc/robot/util/ShooterPhysics.java   | 65 +++++++++++++++++++
 1 file changed, 65 insertions(+)
 create mode 100644 src/main/java/frc/robot/util/ShooterPhysics.java

diff --git a/src/main/java/frc/robot/util/ShooterPhysics.java b/src/main/java/frc/robot/util/ShooterPhysics.java
new file mode 100644
index 0000000..30ec95a
--- /dev/null
+++ b/src/main/java/frc/robot/util/ShooterPhysics.java
@@ -0,0 +1,65 @@
+package frc.robot.util;
+
+import java.lang.Math;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+import frc.robot.constants.Constants;
+
+class ShooterPhysics {
+	// pitch in radians, going up from the horizontal
+	// speed in m/s
+	public record TurretState(Rotation2d yaw, double pitch, double speed){};
+
+	public TurretState getShotParams(Translation2d initialVelocity, Translation2d robot, Translation3d target, double height) {
+		Translation3d robotToTarget = target.minus(new Translation3d(robot));
+		Translation3d impulse = getRequiredImpulse(initialVelocity, robotToTarget, height);
+
+		Translation2d as2d = impulse.toTranslation2d();
+		Rotation2d yaw = as2d.getAngle();
+		double magnitude2d = as2d.getDistance(Translation2d.kZero);
+
+		double pitch = Math.atan2(impulse.getZ(), magnitude2d) + Math.PI * 3 / 2;
+		pitch %= Math.PI * 2;
+		double speed = impulse.getDistance(Translation3d.kZero);
+
+		return new TurretState(yaw, pitch, speed);
+	}
+
+	// assumes shot from (0, 0, 0)
+	private 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
+		// t_vertex = -v_z_impulse / -g
+		// t_vertex = v_z_impulse / g
+		// z_vertex = v_z_impulse * (v_z_impulse / g) - .5 * g * (v_z_impulse / g)²
+		// peakZ = v_z_impulse² / g - .5 * v_z_impulse² / g
+		// peakZ = .5 * v_z_impulse² / g
+		// v_z_impulse² = 2 * peakZ * g
+		// v_z_impulse = √(2 * peakZ * g)
+		double zImpulse = Math.sqrt(2 * peakZ * Constants.GRAVITY_ACCELERATION);
+
+		// now we need time to hit target
+		// z_target = v_z_impulse * t - .5 * g * t²
+		// 0 = -.5 * g * t² + v_z_impulse * t - z_target
+		// quadratic formula
+		// t = (v_z_impulse ± √(4 * (-.5 * g) * (-z_target))) / (2 * -.5 * g)
+		// t = (v_z_impulse ± √(2 * g * z_target)) / -g
+		// onlz use + because we only want the part where it's coming down
+		double t = (zImpulse + Math.sqrt(2 * Constants.GRAVITY_ACCELERATION * target.getZ()))
+			/ -Constants.GRAVITY_ACCELERATION;
+
+		// calculate x and z impulse
+		// x = (v_x_robot + v_x_impulse) * t
+		// x_target = (v_x_robot + v_x_impulse) * t_target
+		// v_x_robot + v_x_impulse = x_target / t_target
+		// v_x_impulse = x_target / t_target - v_x_robot
+		double xImpulse = target.getX() / t - initialVelocity.getX();
+		// same for y
+		double yImpulse = target.getY() / t - initialVelocity.getY();
+		return new Translation3d(xImpulse, yImpulse, zImpulse);
+	}
+}
+
-- 
2.39.5