Updated Field File

src/main/java/frc/robot/RobotContainer.java

@@ -372,11 +372,11 @@ private void debugSetup() {
     Logger.recordOutput(
         logRoot + "/HPS/Left",
         PoseUtils.getPerpendicularOffsetPose(
-            FieldConstants.blueHPSDriverLeft.pose().toPose2d(), AutoConstants.l2ReefOffsetMeters));
+            FieldConstants.blueHPSDriverLeft.HPS.getPose(), AutoConstants.l2ReefOffsetMeters));
     Logger.recordOutput(
         logRoot + "/HPS/Right",
         PoseUtils.getPerpendicularOffsetPose(
-            FieldConstants.blueHPSDriverRight.pose().toPose2d(), AutoConstants.l2ReefOffsetMeters));
+            FieldConstants.blueHPSDriverRight.HPS.getPose(), AutoConstants.l2ReefOffsetMeters));
 
     driverController
         .back()

src/main/java/frc/robot/auto/Autos.java

@@ -8,7 +8,7 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.commands.DriveToPoseCommand;
 import frc.robot.field.FieldConstants;
-import frc.robot.field.FieldConstants.AprilTagStruct;
+import frc.robot.field.HPSFace;
 import frc.robot.field.HumanPlayerStations;
 import frc.robot.field.ReefFaces;
 import frc.robot.field.ReefPole;
@@ -250,13 +250,13 @@ private Command positionToPole(Supplier<ReefPole> pole, double reefOffsetMeters)
                 pole.get().getPerpendicularOffsetPose(reefOffsetMeters), Rotation2d.kPi));
   }
 
-  private Command positionToHPS(Supplier<AprilTagStruct> hps) {
+  private Command positionToHPS(Supplier<HPSFace> hps) {
     return new DriveToPoseCommand(
         drive,
         false,
         () ->
             PoseUtils.getPerpendicularOffsetPose(
-                hps.get().pose().toPose2d(), AutoConstants.l2ReefOffsetMeters));
+                hps.get().HPS.getPose(), AutoConstants.l2ReefOffsetMeters));
   }
 
   private Command pathAndMode(AutoTrajectory trajectory, SuperStructureModes mode) {

src/main/java/frc/robot/bobot_state/BobotState.java

@@ -111,7 +111,7 @@ public static Trigger humanPlayerShouldThrow() {
     return new Trigger(
         () ->
             PoseUtils.getPerpendicularError(
-                    BobotState.getGlobalPose(), FieldUtils.getClosestHPSTag().pose().toPose2d())
+                    BobotState.getGlobalPose(), FieldUtils.getClosestHPSTag().HPS.getPose())
                 < 0.5);
   }
 
@@ -145,7 +145,7 @@ public void periodic() {
       hpsTracker.update();
 
       String calcLogRoot = logRoot + "HPS/";
-      Logger.recordOutput(calcLogRoot + "Closest Tag", FieldUtils.getClosestHPSTag());
+      Logger.recordOutput(calcLogRoot + "Closest Tag", FieldUtils.getClosestHPSTag().HPS);
       Logger.recordOutput(calcLogRoot + "Distance", BobotState.hpsTracker.getDistanceMeters());
       Logger.recordOutput(
           calcLogRoot + "TargetAngleDeg", hpsTracker.getRotationTarget().getDegrees());

src/main/java/frc/robot/bobot_state/varc/HPSTagTracker.java

@@ -10,7 +10,7 @@ public class HPSTagTracker extends TargetAngleTracker {
   private double distanceMeters = 0.0;
 
   public void update() {
-    Pose2d closestPose = FieldUtils.getClosestHPSTag().pose().toPose2d();
+    Pose2d closestPose = FieldUtils.getClosestHPSTag().HPS.getPose();
     rotationTarget = closestPose.getRotation();
     distanceMeters =
         closestPose.getTranslation().getDistance(BobotState.getGlobalPose().getTranslation());

src/main/java/frc/robot/field/FieldConstants.java

@@ -24,12 +24,13 @@ public static record AprilTagStruct(int fiducialId, Pose3d pose) {}
   /*
    * April Tag Lookup
    */
-  public static final AprilTagStruct blueHPSDriverRight =
-      new AprilTagStruct(12, VisionConstants.fieldLayout.getTagPose(12).get());
-  public static final AprilTagStruct blueHPSDriverLeft =
-      new AprilTagStruct(13, VisionConstants.fieldLayout.getTagPose(13).get());
+  public static final HPSFace blueHPSDriverRight =
+      new HPSFace(new AprilTagStruct(12, VisionConstants.fieldLayout.getTagPose(12).get()));
+  public static final HPSFace blueHPSDriverLeft =
+      new HPSFace(new AprilTagStruct(13, VisionConstants.fieldLayout.getTagPose(13).get()));
 
-  public static final Integer blueProcessor = 16;
+  public static final ProcessorFace blueProcessor =
+      new ProcessorFace(new AprilTagStruct(16, VisionConstants.fieldLayout.getTagPose(16).get()));
   public static final AprilTagStruct blueBarge =
       new AprilTagStruct(14, VisionConstants.fieldLayout.getTagPose(14).get());
 
@@ -47,14 +48,15 @@ public static record AprilTagStruct(int fiducialId, Pose3d pose) {}
   public static final ReefFace blueReefKL =
       new ReefFace(new AprilTagStruct(19, VisionConstants.fieldLayout.getTagPose(19).get()));
 
-  public static final AprilTagStruct redHPSDriverLeft =
-      new AprilTagStruct(1, VisionConstants.fieldLayout.getTagPose(1).get());
-  public static final AprilTagStruct redHPSDriverRight =
-      new AprilTagStruct(2, VisionConstants.fieldLayout.getTagPose(2).get());
+  public static final HPSFace redHPSDriverLeft =
+      new HPSFace(new AprilTagStruct(1, VisionConstants.fieldLayout.getTagPose(1).get()));
+  public static final HPSFace redHPSDriverRight =
+      new HPSFace(new AprilTagStruct(2, VisionConstants.fieldLayout.getTagPose(2).get()));
 
-  public static final Integer redProcessor = 3;
+  public static final ProcessorFace redProcessor =
+      new ProcessorFace(new AprilTagStruct(3, VisionConstants.fieldLayout.getTagPose(3).get()));
   public static final AprilTagStruct redBarge =
-      new AprilTagStruct(4, VisionConstants.fieldLayout.getTagPose(4).get());
+      new AprilTagStruct(5, VisionConstants.fieldLayout.getTagPose(5).get());
 
   public static final ReefFace redReefAB =
       new ReefFace(new AprilTagStruct(7, VisionConstants.fieldLayout.getTagPose(7).get()));
@@ -69,18 +71,11 @@ public static record AprilTagStruct(int fiducialId, Pose3d pose) {}
   public static final ReefFace redReefKL =
       new ReefFace(new AprilTagStruct(6, VisionConstants.fieldLayout.getTagPose(6).get()));
 
-  public static final List<ReefFace> blueReefFaces =
+  public static final List<ReefFace> blueReefTags =
       List.of(blueReefAB, blueReefCD, blueReefEF, blueReefGH, blueReefIJ, blueReefKL);
-  public static final List<ReefFace> redReefFaces =
+  public static final List<ReefFace> redReefTags =
       List.of(redReefAB, redReefCD, redReefEF, redReefGH, redReefIJ, redReefKL);
 
-  public static final List<AprilTagStruct> blueHPSTags =
-      List.of(blueHPSDriverLeft, blueHPSDriverRight);
-  public static final List<AprilTagStruct> redHPSTags =
-      List.of(redHPSDriverLeft, redHPSDriverRight);
-
-  public static final List<AprilTagStruct> blueReefTags =
-      blueReefFaces.stream().map(reefFace -> reefFace.tag).toList();
-  public static final List<AprilTagStruct> redReefTags =
-      redReefFaces.stream().map(reefFace -> reefFace.tag).toList();
+  public static final List<HPSFace> blueHPSTags = List.of(blueHPSDriverLeft, blueHPSDriverRight);
+  public static final List<HPSFace> redHPSTags = List.of(redHPSDriverLeft, redHPSDriverRight);
 }

src/main/java/frc/robot/field/FieldUtils.java

@@ -26,7 +26,7 @@ public static double getFlipped() {
 
   public static ReefFace getClosestReef() {
     List<ReefFace> reefTags =
-        FieldUtils.isBlueAlliance() ? FieldConstants.blueReefFaces : FieldConstants.redReefFaces;
+        FieldUtils.isBlueAlliance() ? FieldConstants.blueReefTags : FieldConstants.redReefTags;
     Translation2d robotTranslation = BobotState.getGlobalPose().getTranslation();
 
     ReefFace closestReef =
@@ -44,31 +44,39 @@ public static ReefFace getClosestReef() {
     return closestReef;
   }
 
-  public static AprilTagStruct getClosestHPSTag() {
-    List<AprilTagStruct> hpsTags =
+  public static HPSFace getClosestHPSTag() {
+    List<HPSFace> hpsTags =
         FieldUtils.isBlueAlliance() ? FieldConstants.blueHPSTags : FieldConstants.redHPSTags;
 
     Translation2d robotTranslation = BobotState.getGlobalPose().getTranslation();
 
-    AprilTagStruct closestTag =
+    HPSFace closestTag =
         hpsTags.stream()
             .reduce(
-                (AprilTagStruct tag1, AprilTagStruct tag2) ->
-                    robotTranslation.getDistance(tag1.pose().getTranslation().toTranslation2d())
+                (HPSFace HPS1, HPSFace HPS2) ->
+                    robotTranslation.getDistance(HPS1.tag.pose().getTranslation().toTranslation2d())
                             < robotTranslation.getDistance(
-                                tag2.pose().getTranslation().toTranslation2d())
-                        ? tag1
-                        : tag2)
+                                HPS2.tag.pose().getTranslation().toTranslation2d())
+                        ? HPS1
+                        : HPS2)
             .get();
 
     return closestTag;
   }
 
-  public static AprilTagStruct getBargeTag() {
-    return FieldUtils.isBlueAlliance() ? FieldConstants.blueBarge : FieldConstants.redBarge;
+  public static ProcessorFace getProcessorFace() {
+    ProcessorFace processorTags =
+        FieldUtils.isBlueAlliance() ? FieldConstants.blueProcessor : FieldConstants.redProcessor;
+    ProcessorFace closestTag = processorTags;
+
+    return closestTag;
   }
 
-  public static List<AprilTagStruct> getReefTags() {
+  public static List<ReefFace> getReefTags() {
     return FieldUtils.isBlueAlliance() ? FieldConstants.blueReefTags : FieldConstants.redReefTags;
   }
+
+  public static AprilTagStruct getBargeTag() {
+    return FieldUtils.isBlueAlliance() ? FieldConstants.blueBarge : FieldConstants.redBarge;
+  }
 }

src/main/java/frc/robot/field/HPSFace.java

@@ -0,0 +1,19 @@
+package frc.robot.field;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.field.FieldConstants.AprilTagStruct;
+import frc.robot.util.PoseUtils;
+
+public class HPSFace {
+  public final AprilTagStruct tag;
+  public final HPSLocation HPS;
+
+  public HPSFace(AprilTagStruct tag) {
+    this.tag = tag;
+    this.HPS = new HPSLocation(tag);
+  }
+
+  public double getPerpendicularError(Pose2d robotPose) {
+    return PoseUtils.getPerpendicularError(robotPose, tag.pose().toPose2d());
+  }
+}

src/main/java/frc/robot/field/HPSLocation.java

@@ -0,0 +1,19 @@
+package frc.robot.field;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.field.FieldConstants.AprilTagStruct;
+import frc.robot.util.PoseUtils;
+
+public record HPSLocation(AprilTagStruct tag) {
+  public Pose2d getPose() {
+    return tag.pose().toPose2d();
+  }
+
+  public double getParallellError(Pose2d robotPose) {
+    return PoseUtils.getParallelError(robotPose, tag.pose().toPose2d());
+  }
+
+  public Pose2d getPerpendicularOffsetPose(double offsetMeters) {
+    return PoseUtils.getPerpendicularOffsetPose(getPose(), offsetMeters);
+  }
+}

src/main/java/frc/robot/field/HumanPlayerStations.java

@@ -1,20 +1,18 @@
 package frc.robot.field;
 
-import frc.robot.field.FieldConstants.AprilTagStruct;
-
 public enum HumanPlayerStations {
   LEFT(FieldConstants.blueHPSDriverLeft, FieldConstants.redHPSDriverLeft),
   RIGHT(FieldConstants.blueHPSDriverRight, FieldConstants.redHPSDriverRight);
 
-  public final AprilTagStruct blue;
-  public final AprilTagStruct red;
+  public final HPSFace blue;
+  public final HPSFace red;
 
-  private HumanPlayerStations(AprilTagStruct blue, AprilTagStruct red) {
+  private HumanPlayerStations(HPSFace blue, HPSFace red) {
     this.blue = blue;
     this.red = red;
   }
 
-  public AprilTagStruct get() {
+  public HPSFace get() {
     return FieldUtils.isBlueAlliance() ? blue : red;
   }
 }

src/main/java/frc/robot/field/ProcessorFace.java

@@ -0,0 +1,19 @@
+package frc.robot.field;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.field.FieldConstants.AprilTagStruct;
+import frc.robot.util.PoseUtils;
+
+public class ProcessorFace {
+  public final AprilTagStruct tag;
+  public final ProcessorLocation processor;
+
+  public ProcessorFace(AprilTagStruct tag) {
+    this.tag = tag;
+    this.processor = new ProcessorLocation(tag);
+  }
+
+  public double getPerpendicularError(Pose2d robotPose) {
+    return PoseUtils.getPerpendicularError(robotPose, tag.pose().toPose2d());
+  }
+}

src/main/java/frc/robot/field/ProcessorLocation.java

@@ -0,0 +1,19 @@
+package frc.robot.field;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import frc.robot.field.FieldConstants.AprilTagStruct;
+import frc.robot.util.PoseUtils;
+
+public record ProcessorLocation(AprilTagStruct tag) {
+  public Pose2d getPose() {
+    return tag.pose().toPose2d();
+  }
+
+  public double getParallellError(Pose2d robotPose) {
+    return PoseUtils.getParallelError(robotPose, tag.pose().toPose2d());
+  }
+
+  public Pose2d getPerpendicularOffsetPose(double offsetMeters) {
+    return PoseUtils.getPerpendicularOffsetPose(getPose(), offsetMeters);
+  }
+}

src/main/java/frc/robot/subsystems/vision/AprilTagIOPhoton.java

@@ -6,8 +6,8 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
-import frc.robot.field.FieldConstants.AprilTagStruct;
 import frc.robot.field.FieldUtils;
+import frc.robot.field.ReefFace;
 import frc.robot.subsystems.vision.VisionConstants.PoseEstimationMethod;
 import java.util.ArrayList;
 import java.util.List;
@@ -31,11 +31,11 @@ public class AprilTagIOPhoton implements AprilTagIO {
   private final PhotonPoseEstimator constrainedEstimator;
 
   private final Supplier<Rotation2d> headingSupplier;
-  private final List<AprilTagStruct> trigConstrainedTargets;
+  private final List<ReefFace> trigConstrainedTargets;
 
   public AprilTagIOPhoton(
       VisionSource source,
-      List<AprilTagStruct> trigConstrainedTargets,
+      List<ReefFace> trigConstrainedTargets,
       Supplier<Rotation2d> headingSupplier) {
     camera = new PhotonCamera(source.name());
 

src/main/java/frc/robot/subsystems/vision/AprilTagIOPhotonSim.java

@@ -2,7 +2,7 @@
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
-import frc.robot.field.FieldConstants.AprilTagStruct;
+import frc.robot.field.ReefFace;
 import java.util.List;
 import java.util.function.Supplier;
 import org.photonvision.simulation.PhotonCameraSim;
@@ -13,7 +13,7 @@ public class AprilTagIOPhotonSim extends AprilTagIOPhoton {
 
   public AprilTagIOPhotonSim(
       VisionSource source,
-      List<AprilTagStruct> constrainedTargets,
+      List<ReefFace> constrainedTargets,
       Supplier<Rotation2d> headingSupplier,
       SimCameraConfig config) {
     super(source, constrainedTargets, headingSupplier);