From a079b4450cbc04b22f87f38c464bdf96b12bda6a Mon Sep 17 00:00:00 2001 From: mixxlto Date: Sat, 14 Feb 2026 09:31:55 -0800 Subject: [PATCH] Update TurretAutoShoot.java --- .../robot/commands/gpm/TurretAutoShoot.java | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) 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 -- 2.39.5