Update TurretAutoShoot.java

diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
index dab7bb39945a7160231b6d6b813f916e5ad522e3..67c737ba30cfe029ae57c2882a7228ac2c683367 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
@@ -36,7 +36,7 @@ public class TurretAutoShoot extends Command {
         double D_y = target.getY() - drivepose.getY();
         double D_x = target.getX() - drivepose.getX();
         fieldAngleRad = Math.atan2(D_y, D_x);
-        double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians()));
+        double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Add 180 because drivetrain is backwards
         turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0,180.0);
 
         System.out.println("Aligning the turn to degree angle: " + turretSetpoint);