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 34cf371103973e532c74d4cc1d0fe3dab3d1324a..9083b86097398d774eee9b7ccfb4daf9a8f09416 100644 (file)
--- 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