From 19ac5cb130b950edf3458367c0ea288f28619fd6 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Sat, 12 Jul 2025 16:51:00 -0400 Subject: [PATCH 1/3] alliance utils --- src/main/java/org/team2342/frc/Constants.java | 7 +- .../java/org/team2342/frc/RobotContainer.java | 3 +- .../frc/subsystems/vision/Vision.java | 7 +- .../vision/VisionIOConstrainedSim.java | 6 +- .../frc/subsystems/vision/VisionIOPhoton.java | 4 +- .../vision/VisionIOPhotonConstrained.java | 8 +-- .../frc/subsystems/vision/VisionIOSim.java | 6 +- .../org/team2342/lib/util/AllianceUtils.java | 70 +++++++++++++++++++ 8 files changed, 93 insertions(+), 18 deletions(-) create mode 100644 src/main/java/org/team2342/lib/util/AllianceUtils.java diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index ab975ed..ad74cbe 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -12,6 +12,7 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import org.team2342.lib.util.AllianceUtils; public final class Constants { public static final Mode CURRENT_MODE = Mode.SIM; @@ -49,8 +50,10 @@ public static final class VisionConstants { 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); + static { + AllianceUtils.setFieldLayout( + AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark)); + } // Basic filtering thresholds public static final double MAX_AMBIGUITY = 0.1; diff --git a/src/main/java/org/team2342/frc/RobotContainer.java b/src/main/java/org/team2342/frc/RobotContainer.java index c61e90b..ad38f1d 100644 --- a/src/main/java/org/team2342/frc/RobotContainer.java +++ b/src/main/java/org/team2342/frc/RobotContainer.java @@ -39,6 +39,7 @@ import org.team2342.frc.subsystems.vision.VisionIO; import org.team2342.frc.subsystems.vision.VisionIOPhoton; import org.team2342.frc.subsystems.vision.VisionIOSim; +import org.team2342.lib.util.AllianceUtils; public class RobotContainer { @Getter private final Drive drive; @@ -143,7 +144,7 @@ private void configureBindings() { .whileTrue( new DriveToPose( drive, - VisionConstants.TAG_LAYOUT + AllianceUtils.getFieldLayout() .getTagPose(7) .orElse(new Pose3d()) .toPose2d() diff --git a/src/main/java/org/team2342/frc/subsystems/vision/Vision.java b/src/main/java/org/team2342/frc/subsystems/vision/Vision.java index ade06e4..1c3f69f 100644 --- a/src/main/java/org/team2342/frc/subsystems/vision/Vision.java +++ b/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()); diff --git a/src/main/java/org/team2342/frc/subsystems/vision/VisionIOConstrainedSim.java b/src/main/java/org/team2342/frc/subsystems/vision/VisionIOConstrainedSim.java index ef77da2..5bc705b 100644 --- a/src/main/java/org/team2342/frc/subsystems/vision/VisionIOConstrainedSim.java +++ b/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); } diff --git a/src/main/java/org/team2342/frc/subsystems/vision/VisionIOPhoton.java b/src/main/java/org/team2342/frc/subsystems/vision/VisionIOPhoton.java index 49eb8ad..b7f38a1 100644 --- a/src/main/java/org/team2342/frc/subsystems/vision/VisionIOPhoton.java +++ b/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()); diff --git a/src/main/java/org/team2342/frc/subsystems/vision/VisionIOPhotonConstrained.java b/src/main/java/org/team2342/frc/subsystems/vision/VisionIOPhotonConstrained.java index 0facc82..66d68ce 100644 --- a/src/main/java/org/team2342/frc/subsystems/vision/VisionIOPhotonConstrained.java +++ b/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 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 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 t result.getTargets(), robotToCamera, fieldToRobotSeed, - VisionConstants.TAG_LAYOUT, + AllianceUtils.getFieldLayout(), TargetModel.kAprilTag36h11, headingFree, headingBuffer.getSample(result.getTimestampSeconds()), diff --git a/src/main/java/org/team2342/frc/subsystems/vision/VisionIOSim.java b/src/main/java/org/team2342/frc/subsystems/vision/VisionIOSim.java index 2e169a7..04fe77a 100644 --- a/src/main/java/org/team2342/frc/subsystems/vision/VisionIOSim.java +++ b/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 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); } diff --git a/src/main/java/org/team2342/lib/util/AllianceUtils.java b/src/main/java/org/team2342/lib/util/AllianceUtils.java new file mode 100644 index 0000000..ec79f55 --- /dev/null +++ b/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); + } +} From 63449f91a01db8f39ce51400fe27873003a28fb7 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Thu, 4 Dec 2025 09:11:44 -0500 Subject: [PATCH 2/3] remove reefscape reference --- src/main/java/org/team2342/frc/Constants.java | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/main/java/org/team2342/frc/Constants.java b/src/main/java/org/team2342/frc/Constants.java index ad74cbe..33a47a9 100644 --- a/src/main/java/org/team2342/frc/Constants.java +++ b/src/main/java/org/team2342/frc/Constants.java @@ -6,13 +6,10 @@ 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; import edu.wpi.first.math.util.Units; -import org.team2342.lib.util.AllianceUtils; public final class Constants { public static final Mode CURRENT_MODE = Mode.SIM; @@ -49,12 +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 - static { - AllianceUtils.setFieldLayout( - AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeAndyMark)); - } - // Basic filtering thresholds public static final double MAX_AMBIGUITY = 0.1; public static final double MAX_Z_ERROR = 0.75; From 2acd9ed38aa3a1a3767f63fb1a141e5aa698e4d7 Mon Sep 17 00:00:00 2001 From: Luddo183 <93882319+Luddo183@users.noreply.github.com> Date: Thu, 4 Dec 2025 09:15:41 -0500 Subject: [PATCH 3/3] replace alliance references --- .../java/org/team2342/frc/commands/DriveCommands.java | 11 +++-------- .../java/org/team2342/frc/commands/DriveToPose.java | 7 ++----- .../team2342/frc/commands/RotationLockedDrive.java | 7 ++----- .../java/org/team2342/frc/subsystems/drive/Drive.java | 8 +++----- 4 files changed, 10 insertions(+), 23 deletions(-) diff --git a/src/main/java/org/team2342/frc/commands/DriveCommands.java b/src/main/java/org/team2342/frc/commands/DriveCommands.java index 2210ec6..c9bb577 100644 --- a/src/main/java/org/team2342/frc/commands/DriveCommands.java +++ b/src/main/java/org/team2342/frc/commands/DriveCommands.java @@ -17,8 +17,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; @@ -30,6 +28,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; @@ -86,9 +85,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, @@ -137,9 +134,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, diff --git a/src/main/java/org/team2342/frc/commands/DriveToPose.java b/src/main/java/org/team2342/frc/commands/DriveToPose.java index 3fe7501..aa529d5 100644 --- a/src/main/java/org/team2342/frc/commands/DriveToPose.java +++ b/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( diff --git a/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java b/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java index c87d98b..74ef47a 100644 --- a/src/main/java/org/team2342/frc/commands/RotationLockedDrive.java +++ b/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( diff --git a/src/main/java/org/team2342/frc/subsystems/drive/Drive.java b/src/main/java/org/team2342/frc/subsystems/drive/Drive.java index 6d7a11f..b720fdb 100644 --- a/src/main/java/org/team2342/frc/subsystems/drive/Drive.java +++ b/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); } }