Talon540Programming
diff --git a/‎src/main/java/frc/robot/Constants.java
+2 b/‎src/main/java/frc/robot/Constants.java
+2
diff --git a/‎src/main/java/frc/robot/FieldConstants.java
+87-108 b/‎src/main/java/frc/robot/FieldConstants.java
+87-108
@@ -9,6 +9,8 @@ public class Constants {
   public static final boolean TUNING_MODE = true;
   // Disable the AdvantageKit logger from running
   public static final boolean ENABLE_LOGGING = true;
+  // Disable LEDs, will reduce software and electrical overhead but disable hardware alerts
+  public static final boolean ENABLE_LEDs = false;
 
   public static final double kLoopPeriodSecs = 0.02;
 
 
@@ -1,25 +1,32 @@
 package frc.robot;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.math.util.Units;
-import java.util.ArrayList;
-import java.util.HashMap;
-import java.util.List;
-import java.util.Map;
+import java.io.IOException;
+import java.util.*;
+import lombok.Getter;
 
 /**
  * Contains various field dimensions and useful reference points. All units are in meters and poses
  * have a blue alliance origin.
  */
 public class FieldConstants {
-  public static final double fieldLength = Units.inchesToMeters(690.876);
-  public static final double fieldWidth = Units.inchesToMeters(317);
+  public static AprilTagFieldLayout fieldLayout = AprilTagLayoutType.OFFICIAL.getFieldLayout();
+
+  public static final double fieldLength =
+      AprilTagLayoutType.OFFICIAL.getFieldLayout().getFieldLength();
+  public static final double fieldWidth =
+      AprilTagLayoutType.OFFICIAL.getFieldLayout().getFieldWidth();
   public static final double startingLineX =
       Units.inchesToMeters(299.438); // Measured from the inside of starting line
 
   public static class Processor {
     public static final Pose2d centerFace =
-        new Pose2d(Units.inchesToMeters(235.726), 0, Rotation2d.fromDegrees(90));
+        new Pose2d(
+            AprilTagLayoutType.OFFICIAL.getFieldLayout().getTagPose(16).get().getX(),
+            0,
+            Rotation2d.fromDegrees(90));
   }
 
   public static class Barge {
@@ -36,73 +43,76 @@ public static class Barge {
   }
 
   public static class CoralStation {
-    public static final Pose2d leftCenterFace =
-        new Pose2d(
-            Units.inchesToMeters(33.526),
-            Units.inchesToMeters(291.176),
-            Rotation2d.fromDegrees(90 - 144.011));
+    public static final double stationLength = Units.inchesToMeters(79.750);
     public static final Pose2d rightCenterFace =
         new Pose2d(
             Units.inchesToMeters(33.526),
             Units.inchesToMeters(25.824),
             Rotation2d.fromDegrees(144.011 - 90));
+    public static final Pose2d leftCenterFace =
+        new Pose2d(
+            rightCenterFace.getX(),
+            fieldWidth - rightCenterFace.getY(),
+            Rotation2d.fromRadians(-rightCenterFace.getRotation().getRadians()));
+  }
+
+  public enum ReefLevel {
+    L1(Units.inchesToMeters(25.0), 0),
+    L2(Units.inchesToMeters(31.875 - Math.cos(Math.toRadians(35.0)) * 0.625), -35),
+    L3(Units.inchesToMeters(47.625 - Math.cos(Math.toRadians(35.0)) * 0.625), -35),
+    L4(Units.inchesToMeters(72), -90);
+
+    public final double height;
+    public final double pitch;
+
+    ReefLevel(double height, double pitch) {
+      this.height = height;
+      this.pitch = pitch; // Degrees
+    }
+
+    public static ReefLevel fromLevel(int level) {
+      return Arrays.stream(values())
+          .filter(height -> height.ordinal() == level)
+          .findFirst()
+          .orElse(L4);
+    }
   }
 
   public static class Reef {
+    public static final double faceLength = Units.inchesToMeters(36.792600);
     public static final Translation2d center =
-        new Translation2d(Units.inchesToMeters(176.746), Units.inchesToMeters(158.501));
+        new Translation2d(Units.inchesToMeters(176.746), fieldWidth / 2.0);
     public static final double faceToZoneLine =
         Units.inchesToMeters(12); // Side of the reef to the inside of the reef zone line
 
     public static final Pose2d[] centerFaces =
         new Pose2d[6]; // Starting facing the driver station in clockwise order
-    public static final List<Map<ReefHeight, Pose3d>> branchPositions =
+    public static final List<Map<ReefLevel, Pose3d>> branchPositions =
         new ArrayList<>(); // Starting at the right branch facing the driver station in clockwise
+    public static final List<Map<ReefLevel, Pose2d>> branchPositions2d = new ArrayList<>();
 
     static {
       // Initialize faces
-      centerFaces[0] =
-          new Pose2d(
-              Units.inchesToMeters(144.003),
-              Units.inchesToMeters(158.500),
-              Rotation2d.fromDegrees(180));
-      centerFaces[1] =
-          new Pose2d(
-              Units.inchesToMeters(160.373),
-              Units.inchesToMeters(186.857),
-              Rotation2d.fromDegrees(120));
-      centerFaces[2] =
-          new Pose2d(
-              Units.inchesToMeters(193.116),
-              Units.inchesToMeters(186.858),
-              Rotation2d.fromDegrees(60));
-      centerFaces[3] =
-          new Pose2d(
-              Units.inchesToMeters(209.489),
-              Units.inchesToMeters(158.502),
-              Rotation2d.fromDegrees(0));
-      centerFaces[4] =
-          new Pose2d(
-              Units.inchesToMeters(193.118),
-              Units.inchesToMeters(130.145),
-              Rotation2d.fromDegrees(-60));
-      centerFaces[5] =
-          new Pose2d(
-              Units.inchesToMeters(160.375),
-              Units.inchesToMeters(130.144),
-              Rotation2d.fromDegrees(-120));
+      var aprilTagLayout = AprilTagLayoutType.OFFICIAL.getFieldLayout();
+      centerFaces[0] = aprilTagLayout.getTagPose(18).get().toPose2d();
+      centerFaces[1] = aprilTagLayout.getTagPose(19).get().toPose2d();
+      centerFaces[2] = aprilTagLayout.getTagPose(20).get().toPose2d();
+      centerFaces[3] = aprilTagLayout.getTagPose(21).get().toPose2d();
+      centerFaces[4] = aprilTagLayout.getTagPose(22).get().toPose2d();
+      centerFaces[5] = aprilTagLayout.getTagPose(17).get().toPose2d();
 
       // Initialize branch positions
       for (int face = 0; face < 6; face++) {
-        Map<ReefHeight, Pose3d> fillRight = new HashMap<>();
-        Map<ReefHeight, Pose3d> fillLeft = new HashMap<>();
-        for (var level : ReefHeight.values()) {
+        Map<ReefLevel, Pose3d> fillRight = new HashMap<>();
+        Map<ReefLevel, Pose3d> fillLeft = new HashMap<>();
+        Map<ReefLevel, Pose2d> fillRight2d = new HashMap<>();
+        Map<ReefLevel, Pose2d> fillLeft2d = new HashMap<>();
+        for (var level : ReefLevel.values()) {
           Pose2d poseDirection = new Pose2d(center, Rotation2d.fromDegrees(180 - (60 * face)));
           double adjustX = Units.inchesToMeters(30.738);
           double adjustY = Units.inchesToMeters(6.469);
 
-          fillRight.put(
-              level,
+          var rightBranchPose =
               new Pose3d(
                   new Translation3d(
                       poseDirection
@@ -115,9 +125,8 @@ public static class Reef {
                   new Rotation3d(
                       0,
                       Units.degreesToRadians(level.pitch),
-                      poseDirection.getRotation().getRadians())));
-          fillLeft.put(
-              level,
+                      poseDirection.getRotation().getRadians()));
+          var leftBranchPose =
               new Pose3d(
                   new Translation3d(
                       poseDirection
@@ -130,74 +139,44 @@ public static class Reef {
                   new Rotation3d(
                       0,
                       Units.degreesToRadians(level.pitch),
-                      poseDirection.getRotation().getRadians())));
+                      poseDirection.getRotation().getRadians()));
+
+          fillRight.put(level, rightBranchPose);
+          fillLeft.put(level, leftBranchPose);
+          fillRight2d.put(level, rightBranchPose.toPose2d());
+          fillLeft2d.put(level, leftBranchPose.toPose2d());
         }
-        branchPositions.add((face * 2) + 1, fillRight);
-        branchPositions.add((face * 2) + 2, fillLeft);
+        branchPositions.add(fillRight);
+        branchPositions.add(fillLeft);
+        branchPositions2d.add(fillRight2d);
+        branchPositions2d.add(fillLeft2d);
       }
     }
   }
 
   public static class StagingPositions {
     // Measured from the center of the ice cream
-    public static final Pose2d leftIceCream =
-        new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(230.5), new Rotation2d());
+    public static final double separation = Units.inchesToMeters(72.0);
     public static final Pose2d middleIceCream =
-        new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(158.5), new Rotation2d());
+        new Pose2d(Units.inchesToMeters(48), fieldWidth / 2.0, new Rotation2d());
+    public static final Pose2d leftIceCream =
+        new Pose2d(Units.inchesToMeters(48), middleIceCream.getY() + separation, new Rotation2d());
     public static final Pose2d rightIceCream =
-        new Pose2d(Units.inchesToMeters(48), Units.inchesToMeters(86.5), new Rotation2d());
+        new Pose2d(Units.inchesToMeters(48), middleIceCream.getY() - separation, new Rotation2d());
   }
 
-  public enum ReefHeight {
-    L4(Units.inchesToMeters(72), -90),
-    L3(Units.inchesToMeters(47.625), -35),
-    L2(Units.inchesToMeters(31.875), -35),
-    L1(Units.inchesToMeters(18), 0);
+  @Getter
+  public enum AprilTagLayoutType {
+    OFFICIAL("2025-reefscape-welded.json");
 
-    ReefHeight(double height, double pitch) {
-      this.height = height;
-      this.pitch = pitch; // in degrees
-    }
+    private final AprilTagFieldLayout fieldLayout;
 
-    public final double height;
-    public final double pitch;
+    private AprilTagLayoutType(String file) {
+      try {
+        fieldLayout = AprilTagFieldLayout.loadFromResource(file);
+      } catch (IOException exception) {
+        throw new RuntimeException("Failed to load AprilTagLayoutType: " + file, exception);
+      }
+    }
   }
-
-  // TODO
-  // public static final double aprilTagWidth = Units.inchesToMeters(6.50);
-  // public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL;
-  // public static final int aprilTagCount = 22;
-  //
-  // @Getter
-  // public enum AprilTagLayoutType {
-  //   OFFICIAL("2025-official");
-  //
-  //   AprilTagLayoutType(String name) {
-  //     if (Constants.disableHAL) {
-  //       layout = null;
-  //     } else {
-  //       try {
-  //         layout =
-  //             new AprilTagFieldLayout(
-  //                 Path.of(Filesystem.getDeployDirectory().getPath(), "apriltags", name +
-  // ".json"));
-  //       } catch (IOException e) {
-  //         throw new RuntimeException(e);
-  //       }
-  //     }
-  //     if (layout == null) {
-  //       layoutString = "";
-  //     } else {
-  //       try {
-  //         layoutString = new ObjectMapper().writeValueAsString(layout);
-  //       } catch (JsonProcessingException e) {
-  //         throw new RuntimeException(
-  //             "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar");
-  //       }
-  //     }
-  //   }
-  //
-  //   private final AprilTagFieldLayout layout;
-  //   private final String layoutString;
-  // }
 }