Much better Turret PID

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;
+    }
+
+}

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 turretAngle = Utils.calculateTurretAngleFromCameraTagDistance(targetsPose[2]) * Constants.ShooterConstants.TURRET_GEAR_RATIO;
-                double cameraToShootDistance = 0.26035;
+                turretSubsystem.setTurretMotorRotation(-turretAngle);
-                // 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);
 
                 driveSubsystem.rotateChassis(-cameraAutoCenterController.calculate(targetsPose[0])); // This may need to be negated
             }

src/main/java/frc/robot/subsystems/TurretSubsystem.java

@@ -74,10 +74,10 @@ public class TurretSubsystem extends TestableSubsystem {
       new TalonFXConfiguration()
         .withSlot0(
           new Slot0Configs()
-            .withKP(3)
+            .withKP(6)
-            .withKI(1.5)
+            .withKI(0)
-            .withKD(0)
+            .withKD(0.3)
-            .withKS(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)