From 317d21d0003af669c10ee2071364a2a52a85af23 Mon Sep 17 00:00:00 2001
From: Tyler Wilcox <tyler@tywilly.com>
Date: Sat, 4 Apr 2026 13:38:28 -0400
Subject: [PATCH] Much better Turret PID

---
 src/main/java/frc/robot/Utils.java            | 25 +++++++++++++++++++
 .../AutoShootCommandChassisTurret.java        | 15 +++--------
 .../frc/robot/subsystems/TurretSubsystem.java | 10 ++++----
 3 files changed, 33 insertions(+), 17 deletions(-)
 create mode 100644 src/main/java/frc/robot/Utils.java

diff --git a/src/main/java/frc/robot/Utils.java b/src/main/java/frc/robot/Utils.java
new file mode 100644
index 0000000..64bd85f
--- /dev/null
+++ b/src/main/java/frc/robot/Utils.java
@@ -0,0 +1,25 @@
+package frc.robot;
+
+public class Utils {
+    
+    /**
+     * Calculates the needed angle of the turret for a distance from the HUB.
+     * @param distanceToTag
+     * @return number of rotations of the turret, robot relative
+     */
+    public static double calculateTurretAngleFromCameraTagDistance(double distanceToTag){
+
+        // Calculate turret Angle
+        double cameraDistanceToTarget = distanceToTag + .6;
+        double cameraToShootDistance = 0.26035;
+        // a^2 + b^2 = c^2
+        double shooterDistanceToTarget = Math
+                .sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2));
+        double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget);
+        double robotTurretAngle = (Math.PI / 2) - angleAtShooter;
+        double robotTurretRotations = (robotTurretAngle / Math.PI);
+
+        return robotTurretRotations;
+    }
+
+}
diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
index 629bfe5..0b6d64d 100644
--- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
+++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java
@@ -23,6 +23,7 @@ import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
+import frc.robot.Utils;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
@@ -117,18 +118,8 @@ public class AutoShootCommandChassisTurret extends Command{
                 Logger.recordOutput("AutoAlign-Target Distance", targetsPose[2]);
 
                 // Calculate turret Angle
-                double cameraDistanceToTarget = targetsPose[2];
-                double cameraToShootDistance = 0.26035;
-                // a^2 + b^2 = c^2
-                double shooterDistanceToTarget = Math.sqrt(Math.pow(cameraDistanceToTarget, 2) + Math.pow(cameraToShootDistance, 2));
-                double angleAtShooter = Math.acos(cameraToShootDistance / shooterDistanceToTarget);
-                Logger.recordOutput("AutoAlign-TurretAngleOfTriagle", angleAtShooter);
-                double robotTurretAngle = (Math.PI/2) - angleAtShooter;
-                Logger.recordOutput("AutoAlign-TurretAngleRelativeToRobot", robotTurretAngle);
-                double robotTurretRotations = (robotTurretAngle / Math.PI) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
-                Logger.recordOutput("AutoAlign-TurretRotations", robotTurretRotations);
-
-                turretSubsystem.setTurretMotorRotation(-robotTurretRotations);
+                double turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
+                turretSubsystem.setTurretMotorRotation(-turretAngle);
 
                 driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
             }
diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java
index 95c3437..3391db5 100644
--- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java
@@ -74,10 +74,10 @@ public class TurretSubsystem extends TestableSubsystem {
       new TalonFXConfiguration()
         .withSlot0(
           new Slot0Configs()
-            .withKP(3)
-            .withKI(1.5)
-            .withKD(0)
-            .withKS(0.3)
+            .withKP(6)
+            .withKI(0)
+            .withKD(0.3)
+            .withKS(0)
         )
         .withMotorOutput(
           new MotorOutputConfigs()
@@ -131,7 +131,7 @@ public class TurretSubsystem extends TestableSubsystem {
   public boolean isLauncherUpToSpeed() {
     // Valid if velcity is within 10% of setpoint.
     // May need to lower this
-    return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.075;
+    return Math.abs(launcherControler.getSetpoint() - launcherMotor.getEncoder().getVelocity()) >= launcherControler.getSetpoint() * 0.03;
   }
 
   public void setTargetElevation(double elev)