From 2f981f2d09fccad6f54808bfdeb367cdf8215db0 Mon Sep 17 00:00:00 2001 From: Tyler Wilcox Date: Thu, 2 Apr 2026 19:42:26 -0400 Subject: [PATCH] Hopefully working auto align --- .../robot/commands/AutoShootCommandChassisTurret.java | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java index a524d0e..0ceeee2 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java +++ b/src/main/java/frc/robot/commands/AutoShootCommandChassisTurret.java @@ -59,7 +59,7 @@ public class AutoShootCommandChassisTurret extends Command{ cameraAutoCenterController.setSetpoint(0); - deadreckoningAutoCenterController = new PIDController(Math.PI * 0.1, 0, 0.0); + deadreckoningAutoCenterController = new PIDController(Math.PI, 0, 0.0); deadreckoningAutoCenterController.setTolerance(0.2); // Units is rotations deadreckoningAutoCenterController.enableContinuousInput(-Math.PI, Math.PI); } @@ -92,12 +92,7 @@ public class AutoShootCommandChassisTurret extends Command{ Translation2d delta = botPose.getTranslation().toTranslation2d().minus(closestTag.getTranslation().toTranslation2d()); Logger.recordOutput("AutoAlign-ClosestAprilTagTransform", delta); - DriverStation.getAlliance().ifPresent((aliance) -> { - if (aliance == Alliance.Blue) { - // Add 180 because we want the back of the robot to face the HUB - deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.k180deg).getRadians()); - } - }); + deadreckoningAutoCenterController.setSetpoint(delta.getAngle().plus(Rotation2d.kZero).getRadians()); } @Override @@ -111,7 +106,7 @@ public class AutoShootCommandChassisTurret extends Command{ Logger.recordOutput("AutoAlignState-Tracking", "Deadreckoning"); Pose3d botPose = driveSubsystem.getBotPose(); - driveSubsystem.rotateChassis(deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); + driveSubsystem.rotateChassis(-deadreckoningAutoCenterController.calculate(botPose.getRotation().toRotation2d().getRadians())); } else // We have an april tag, center it to the camera frame {