From: mixxlto <maxtan0626@gmail.com>
Date: Sat, 14 Feb 2026 17:31:55 +0000 (-0800)
Subject: Update TurretAutoShoot.java
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=a079b4450cbc04b22f87f38c464bdf96b12bda6a;p=FRC2026.git

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 34cf371..9083b86 100644
--- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java
@@ -38,7 +38,7 @@ public class TurretAutoShoot extends Command {
     public TurretAutoShoot(Turret turret, Drivetrain drivetrain) {
         this.turret = turret;
         this.drivetrain = drivetrain;
-        drivepose  = drivetrain.getPose().rotateBy(new Rotation2d(Math.PI));
+        drivepose  = new Pose2d(drivepose.getTranslation(), drivepose.getRotation().plus(new Rotation2d(Math.PI)));
         
         addRequirements(turret);
     }
@@ -98,13 +98,18 @@ public class TurretAutoShoot extends Command {
     }
 
     public void updateDrivePose(){
+        Pose2d currentPose = drivetrain.getPose();
+        // Add 180 degrees to the rotation bc robot is backwards
+        drivepose = new Pose2d(
+            currentPose.getTranslation(), 
+            currentPose.getRotation().plus(new Rotation2d(Math.PI))
+        );
         ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
-        drivepose = drivetrain.getPose().rotateBy(new Rotation2d(Math.PI));
-        // .exp(
-        //     new Twist2d(
-        //         robotRelVel.vxMetersPerSecond * phaseDelay,
-        //         robotRelVel.vyMetersPerSecond * phaseDelay,
-        //         robotRelVel.omegaRadiansPerSecond * phaseDelay));
+        drivepose.exp(
+            new Twist2d(
+                robotRelVel.vxMetersPerSecond * phaseDelay,
+                robotRelVel.vyMetersPerSecond * phaseDelay,
+                robotRelVel.omegaRadiansPerSecond * phaseDelay));
     }
 
     @Override