Alliance Utils

src/main/java/org/team2342/frc/Constants.java

@@ -6,8 +6,6 @@
 
 package org.team2342.frc;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation3d;
@@ -48,10 +46,6 @@ public static final class VisionConstants {
                 Units.inchesToMeters(12.125)),
             new Rotation3d(0, 0.0, Units.degreesToRadians(35)));
 
-    // The layout of the AprilTags on the field
-    public static final AprilTagFieldLayout TAG_LAYOUT =
-        AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark);
-
     // Basic filtering thresholds
     public static final double MAX_AMBIGUITY = 0.1;
     public static final double MAX_Z_ERROR = 0.75;

src/main/java/org/team2342/frc/RobotContainer.java

@@ -37,6 +37,7 @@
 import org.team2342.frc.subsystems.vision.Vision;
 import org.team2342.frc.subsystems.vision.VisionIO;
 import org.team2342.frc.subsystems.vision.VisionIOSim;
+import org.team2342.lib.util.AllianceUtils;
 import org.team2342.lib.util.EnhancedXboxController;
 
 public class RobotContainer {
@@ -139,7 +140,7 @@ private void configureBindings() {
         .whileTrue(
             new DriveToPose(
                 drive,
-                VisionConstants.TAG_LAYOUT
+                AllianceUtils.getFieldLayout()
                     .getTagPose(7)
                     .orElse(new Pose3d())
                     .toPose2d()

src/main/java/org/team2342/frc/commands/DriveCommands.java

@@ -16,8 +16,6 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -29,6 +27,7 @@
 import java.util.function.Supplier;
 import org.team2342.frc.Constants.DriveConstants;
 import org.team2342.frc.subsystems.drive.Drive;
+import org.team2342.lib.util.AllianceUtils;
 
 public class DriveCommands {
   private static final double ANGLE_KP = 5.0;
@@ -80,9 +79,7 @@ public static Command joystickDrive(
                   linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
                   linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
                   omega * drive.getMaxAngularSpeedRadPerSec());
-          boolean isFlipped =
-              DriverStation.getAlliance().isPresent()
-                  && DriverStation.getAlliance().get() == Alliance.Red;
+          boolean isFlipped = AllianceUtils.isRedAlliance();
           drive.runVelocity(
               ChassisSpeeds.fromFieldRelativeSpeeds(
                   speeds,
@@ -131,9 +128,7 @@ public static Command joystickDriveAtAngle(
                       linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
                       linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
                       omega);
-              boolean isFlipped =
-                  DriverStation.getAlliance().isPresent()
-                      && DriverStation.getAlliance().get() == Alliance.Red;
+              boolean isFlipped = AllianceUtils.isRedAlliance();
               drive.runVelocity(
                   ChassisSpeeds.fromFieldRelativeSpeeds(
                       speeds,

src/main/java/org/team2342/frc/commands/DriveToPose.java

@@ -17,13 +17,12 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.Logger;
 import org.team2342.frc.subsystems.drive.Drive;
+import org.team2342.lib.util.AllianceUtils;
 
 public class DriveToPose extends Command {
   private PIDController xController = new PIDController(4.0, 0.0, 0.02);
@@ -104,9 +103,7 @@ public void execute() {
 
     ChassisSpeeds speeds = controller.calculate(currentPosition, goal, 0, target.getRotation());
 
-    boolean isFlipped =
-        DriverStation.getAlliance().isPresent()
-            && DriverStation.getAlliance().get() == Alliance.Red;
+    boolean isFlipped = AllianceUtils.isRedAlliance();
 
     Translation2d joystickTranslation =
         DriveCommands.getLinearVelocityFromJoysticks(

src/main/java/org/team2342/frc/commands/RotationLockedDrive.java

@@ -12,14 +12,13 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 import org.team2342.frc.Constants.DriveConstants;
 import org.team2342.frc.subsystems.drive.Drive;
+import org.team2342.lib.util.AllianceUtils;
 
 public class RotationLockedDrive extends Command {
 
@@ -85,9 +84,7 @@ public void execute() {
             xSupplier.getAsDouble(), ySupplier.getAsDouble());
 
     // Convert to field relative speeds & send command
-    boolean isFlipped =
-        DriverStation.getAlliance().isPresent()
-            && DriverStation.getAlliance().get() == Alliance.Red;
+    boolean isFlipped = AllianceUtils.isRedAlliance();
 
     drive.runVelocity(
         ChassisSpeeds.fromFieldRelativeSpeeds(

src/main/java/org/team2342/frc/subsystems/drive/Drive.java

@@ -32,7 +32,6 @@
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -45,6 +44,7 @@
 import org.littletonrobotics.junction.Logger;
 import org.team2342.frc.Constants.DriveConstants;
 import org.team2342.lib.logging.ExecutionLogger;
+import org.team2342.lib.util.AllianceUtils;
 import org.team2342.lib.util.LocalADStarAK;
 import org.team2342.lib.util.SwerveSetpointGenerator;
 import org.team2342.lib.util.SwerveSetpointGenerator.ModuleLimits;
@@ -121,7 +121,7 @@ public Drive(GyroIO gyro, ModuleIO fl, ModuleIO fr, ModuleIO bl, ModuleIO br) {
         new PPHolonomicDriveController(
             new PIDConstants(6.0, 0.0, 0.0), new PIDConstants(8.0, 0.0, 0.1)),
         pathplannerConfig,
-        () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
+        () -> AllianceUtils.isRedAlliance(),
         this);
 
     // Logging callbacks for PathPlanner
@@ -395,9 +395,7 @@ public void initSendable(SendableBuilder builder) {
         "Back Right Velocity", () -> modules[3].getVelocityMetersPerSec(), null);
     builder.addDoubleProperty(
         "Robot Angle",
-        () ->
-            getRotation().getRadians()
-                + (DriverStation.getAlliance().orElse(Alliance.Red) == Alliance.Red ? Math.PI : 0),
+        () -> getRotation().getRadians() + (AllianceUtils.isRedAlliance() ? Math.PI : 0),
         null);
   }
 }

src/main/java/org/team2342/frc/subsystems/vision/Vision.java

@@ -23,6 +23,7 @@
 import org.team2342.frc.Constants.VisionConstants;
 import org.team2342.frc.subsystems.vision.VisionIO.PoseObservationType;
 import org.team2342.lib.logging.ExecutionLogger;
+import org.team2342.lib.util.AllianceUtils;
 
 public class Vision extends SubsystemBase {
   private final VisionConsumer consumer;
@@ -84,7 +85,7 @@ public void periodic() {
 
       // Add tag poses
       for (int tagId : inputs[cameraIndex].tagIds) {
-        var tagPose = VisionConstants.TAG_LAYOUT.getTagPose(tagId);
+        var tagPose = AllianceUtils.getFieldLayout().getTagPose(tagId);
         if (tagPose.isPresent()) {
           tagPoses.add(tagPose.get());
         }
@@ -103,9 +104,9 @@ public void periodic() {
 
                 // Must be within the field boundaries
                 || observation.pose().getX() < 0.0
-                || observation.pose().getX() > VisionConstants.TAG_LAYOUT.getFieldLength()
+                || observation.pose().getX() > AllianceUtils.getFieldLayout().getFieldLength()
                 || observation.pose().getY() < 0.0
-                || observation.pose().getY() > VisionConstants.TAG_LAYOUT.getFieldWidth();
+                || observation.pose().getY() > AllianceUtils.getFieldLayout().getFieldWidth();
 
         // Add pose to log
         robotPoses.add(observation.pose());

src/main/java/org/team2342/frc/subsystems/vision/VisionIOConstrainedSim.java

@@ -17,7 +17,7 @@
 import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.simulation.SimCameraProperties;
 import org.photonvision.simulation.VisionSystemSim;
-import org.team2342.frc.Constants.VisionConstants;
+import org.team2342.lib.util.AllianceUtils;
 
 public class VisionIOConstrainedSim extends VisionIOPhotonConstrained {
   private static VisionSystemSim visionSim;
@@ -44,13 +44,13 @@ public VisionIOConstrainedSim(
     // Initialize vision sim
     if (visionSim == null) {
       visionSim = new VisionSystemSim("main");
-      visionSim.addAprilTags(VisionConstants.TAG_LAYOUT);
+      visionSim.addAprilTags(AllianceUtils.getFieldLayout());
     }
 
     // Add sim camera
     var cameraProperties = new SimCameraProperties();
     cameraProperties.setCalibration(800, 600, cameraMatrix, distCoeffs);
-    cameraSim = new PhotonCameraSim(camera, cameraProperties, VisionConstants.TAG_LAYOUT);
+    cameraSim = new PhotonCameraSim(camera, cameraProperties, AllianceUtils.getFieldLayout());
     visionSim.addCamera(cameraSim, robotToCamera);
   }
 

src/main/java/org/team2342/frc/subsystems/vision/VisionIOPhoton.java

@@ -15,7 +15,7 @@
 import java.util.List;
 import java.util.Set;
 import org.photonvision.PhotonCamera;
-import org.team2342.frc.Constants.VisionConstants;
+import org.team2342.lib.util.AllianceUtils;
 
 /** IO implementation for real PhotonVision hardware. */
 public class VisionIOPhoton implements VisionIO {
@@ -83,7 +83,7 @@ public void updateInputs(VisionIOInputs inputs) {
         var target = result.targets.get(0);
 
         // Calculate robot pose
-        var tagPose = VisionConstants.TAG_LAYOUT.getTagPose(target.fiducialId);
+        var tagPose = AllianceUtils.getFieldLayout().getTagPose(target.fiducialId);
         if (tagPose.isPresent()) {
           Transform3d fieldToTarget =
               new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation());

src/main/java/org/team2342/frc/subsystems/vision/VisionIOPhotonConstrained.java

@@ -43,7 +43,7 @@
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
-import org.team2342.frc.Constants.VisionConstants;
+import org.team2342.lib.util.AllianceUtils;
 
 /** IO implementation for real PhotonVision hardware. */
 public class VisionIOPhotonConstrained implements VisionIO {
@@ -154,7 +154,7 @@ private PoseObservation basic(PhotonPipelineResult result, Set<Short> tagIds) {
     var target = result.targets.get(0);
 
     // Calculate robot pose
-    var tagPose = VisionConstants.TAG_LAYOUT.getTagPose(target.fiducialId);
+    var tagPose = AllianceUtils.getFieldLayout().getTagPose(target.fiducialId);
     if (tagPose.isPresent()) {
       Transform3d fieldToTarget =
           new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation());
@@ -212,7 +212,7 @@ private PoseObservation constrainedPNP(PhotonPipelineResult result, Set<Short> t
       tagDist = totalTagDistance / result.targets.size(); // Average tag distance
     } else {
       var target = result.targets.get(0);
-      var tagPose = VisionConstants.TAG_LAYOUT.getTagPose(target.fiducialId);
+      var tagPose = AllianceUtils.getFieldLayout().getTagPose(target.fiducialId);
       if (tagPose.isPresent()) {
         Transform3d fieldToTarget =
             new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation());
@@ -249,7 +249,7 @@ private PoseObservation constrainedPNP(PhotonPipelineResult result, Set<Short> t
             result.getTargets(),
             robotToCamera,
             fieldToRobotSeed,
-            VisionConstants.TAG_LAYOUT,
+            AllianceUtils.getFieldLayout(),
             TargetModel.kAprilTag36h11,
             headingFree,
             headingBuffer.getSample(result.getTimestampSeconds()),

src/main/java/org/team2342/frc/subsystems/vision/VisionIOSim.java

@@ -13,7 +13,7 @@
 import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.simulation.SimCameraProperties;
 import org.photonvision.simulation.VisionSystemSim;
-import org.team2342.frc.Constants.VisionConstants;
+import org.team2342.lib.util.AllianceUtils;
 
 /** IO implementation for physics sim using PhotonVision simulator. */
 public class VisionIOSim extends VisionIOPhoton {
@@ -35,12 +35,12 @@ public VisionIOSim(String name, Transform3d robotToCamera, Supplier<Pose2d> pose
     // Initialize vision sim
     if (visionSim == null) {
       visionSim = new VisionSystemSim("main");
-      visionSim.addAprilTags(VisionConstants.TAG_LAYOUT);
+      visionSim.addAprilTags(AllianceUtils.getFieldLayout());
     }
 
     // Add sim camera
     var cameraProperties = new SimCameraProperties();
-    cameraSim = new PhotonCameraSim(camera, cameraProperties, VisionConstants.TAG_LAYOUT);
+    cameraSim = new PhotonCameraSim(camera, cameraProperties, AllianceUtils.getFieldLayout());
     visionSim.addCamera(cameraSim, robotToCamera);
   }
 

src/main/java/org/team2342/lib/util/AllianceUtils.java

@@ -0,0 +1,70 @@
+// Copyright (c) 2025 Team 2342
+// https://github.com/FRCTeamPhoenix
+//
+// This source code is licensed under the MIT License.
+// See the LICENSE file in the root directory of this project.
+
+package org.team2342.lib.util;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import lombok.Getter;
+import lombok.Setter;
+
+/** Class with alliance-related utility functions */
+public class AllianceUtils {
+
+  @Getter @Setter
+  private static AprilTagFieldLayout fieldLayout =
+      AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+
+  public static boolean isRedAlliance() {
+    var alliance = DriverStation.getAlliance();
+
+    if (alliance.isEmpty())
+      DriverStation.reportError("Alliance variable is empty, defaulting to red!", false);
+    return alliance.orElse(Alliance.Red) == Alliance.Red;
+  }
+
+  public static boolean isBlueAlliance() {
+    return !isRedAlliance();
+  }
+
+  public static Trigger driverStationAttachedTrigger() {
+    return new Trigger(DriverStation::isDSAttached);
+  }
+
+  public static Pose2d flipToAlliance(Pose2d bluePose, AprilTagFieldLayout field) {
+    return isRedAlliance()
+        ? new Pose2d(
+            field.getFieldLength() - bluePose.getX(),
+            field.getFieldWidth() - bluePose.getY(),
+            bluePose.getRotation().rotateBy(Rotation2d.kPi))
+        : bluePose;
+  }
+
+  public static Pose3d flipToAlliance(Pose3d bluePose, AprilTagFieldLayout field) {
+    return isRedAlliance()
+        ? new Pose3d(
+            field.getFieldLength() - bluePose.getX(),
+            field.getFieldWidth() - bluePose.getY(),
+            bluePose.getZ(),
+            bluePose.getRotation().rotateBy(new Rotation3d(0.0, 0.0, Math.PI)))
+        : bluePose;
+  }
+
+  public static Pose2d flipToAlliance(Pose2d bluePose) {
+    return flipToAlliance(bluePose, fieldLayout);
+  }
+
+  public static Pose3d flipToAlliance(Pose3d bluePose) {
+    return flipToAlliance(bluePose, fieldLayout);
+  }
+}