From 02774123ca6d6d9c0ae6b4b13ebe45ae49218e30 Mon Sep 17 00:00:00 2001 From: kacperowyy Date: Thu, 19 Mar 2026 15:34:17 +0100 Subject: [PATCH] Adjust aim depending on alliance --- src/main/java/frc/robot/subsystems/Vision/Aim.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/Vision/Aim.java b/src/main/java/frc/robot/subsystems/Vision/Aim.java index 5450328..4b8d3a8 100644 --- a/src/main/java/frc/robot/subsystems/Vision/Aim.java +++ b/src/main/java/frc/robot/subsystems/Vision/Aim.java @@ -103,6 +103,10 @@ public Pose2d findPoseForShoot() { // Rotation facing toward the tag Rotation2d targetRotation = new Rotation2d(toTag.getX(), toTag.getY()); + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + targetRotation = targetRotation.plus(Rotation2d.fromDegrees(180)); + } Pose2d shootingPose = new Pose2d(targetTranslation, targetRotation);