AutoAlign Cleanup

Author: iamawesomecat

src/main/java/frc/robot/subsystems/auto/AutoAlign.java

@@ -182,6 +182,75 @@ public static boolean poseInPlace(AlignType type) {
   private static final Pose2d rRedReefFaceKL =
       aprilTagFieldLayout.getTagPose(6).get().toPose2d().plus(l1RightOfReef);
 
+  private static final List allBlueBranches =
+      List.of(
+          blueBranchA,
+          blueBranchB,
+          blueBranchC,
+          blueBranchD,
+          blueBranchE,
+          blueBranchF,
+          blueBranchG,
+          blueBranchH,
+          blueBranchI,
+          blueBranchJ,
+          blueBranchK,
+          blueBranchL);
+  private static final List allRedBranches =
+      List.of(
+          redBranchA,
+          redBranchB,
+          redBranchC,
+          redBranchD,
+          redBranchE,
+          redBranchF,
+          redBranchG,
+          redBranchH,
+          redBranchI,
+          redBranchJ,
+          redBranchK,
+          redBranchL);
+  private static final List leftBlueBranches =
+      List.of(blueBranchA, blueBranchC, blueBranchE, blueBranchG, blueBranchI, blueBranchK);
+  private static final List leftRedBranches =
+      List.of(redBranchA, redBranchC, redBranchE, redBranchG, redBranchI, redBranchK);
+  private static final List rightBlueBranches =
+      List.of(blueBranchB, blueBranchD, blueBranchF, blueBranchH, blueBranchJ, blueBranchL);
+  private static final List rightRedBranches =
+      List.of(redBranchB, redBranchD, redBranchF, redBranchH, redBranchJ, redBranchL);
+  private static final List leftL1BluePoses =
+      List.of(
+          lBlueReefFaceAB,
+          lBlueReefFaceCD,
+          lBlueReefFaceEF,
+          lBlueReefFaceGH,
+          lBlueReefFaceIJ,
+          lBlueReefFaceKL);
+  private static final List leftL1RedPoses =
+      List.of(
+          lRedReefFaceAB,
+          lRedReefFaceCD,
+          lRedReefFaceEF,
+          lRedReefFaceGH,
+          lRedReefFaceIJ,
+          lRedReefFaceKL);
+  private static final List rightL1BluePoses =
+      List.of(
+          rBlueReefFaceAB,
+          rBlueReefFaceCD,
+          rBlueReefFaceEF,
+          rBlueReefFaceGH,
+          rBlueReefFaceIJ,
+          rBlueReefFaceKL);
+  private static final List rightL1RedPoses =
+      List.of(
+          rRedReefFaceAB,
+          rRedReefFaceCD,
+          rRedReefFaceEF,
+          rRedReefFaceGH,
+          rRedReefFaceIJ,
+          rRedReefFaceKL);
+
   private static class AutoAlignCommand extends Command {
 
     protected final PIDController pidX = new PIDController(4, 0, 0);
@@ -256,102 +325,17 @@ public void end(boolean interrupted) {
 
     public static Pose2d getTargetPose(Pose2d pose, AlignType type) {
       return switch (type) {
-        case LEFTB -> getNearestLeftBranch(pose);
-        case RIGHTB -> getNearestRightBranch(pose);
-        case L1LB -> getNearestL1L(pose);
-        case L1RB -> getNearestL1R(pose);
-        case ALLB -> getNearestBranch(pose);
+        case LEFTB -> getNearestBranch(pose, leftBlueBranches, leftRedBranches);
+        case RIGHTB -> getNearestBranch(pose, rightBlueBranches, rightRedBranches);
+        case L1LB -> getNearestBranch(pose, leftL1BluePoses, leftL1RedPoses);
+        case L1RB -> getNearestBranch(pose, rightL1BluePoses, rightL1RedPoses);
+        case ALLB -> getNearestBranch(pose, allBlueBranches, allRedBranches);
       };
     }
 
-    private static Pose2d getNearestBranch(Pose2d p) {
-      List<Pose2d> branchPose2ds =
-          AllianceUtils.isBlue()
-              ? List.of(
-                  blueBranchA,
-                  blueBranchB,
-                  blueBranchC,
-                  blueBranchD,
-                  blueBranchE,
-                  blueBranchF,
-                  blueBranchG,
-                  blueBranchH,
-                  blueBranchI,
-                  blueBranchJ,
-                  blueBranchK,
-                  blueBranchL)
-              : List.of(
-                  redBranchA,
-                  redBranchB,
-                  redBranchC,
-                  redBranchD,
-                  redBranchE,
-                  redBranchF,
-                  redBranchG,
-                  redBranchH,
-                  redBranchI,
-                  redBranchJ,
-                  redBranchK,
-                  redBranchL);
-      return p.nearest(branchPose2ds);
-    }
-
-    private static Pose2d getNearestLeftBranch(Pose2d p) {
-      List<Pose2d> branchPose2ds =
-          AllianceUtils.isBlue()
-              ? List.of(
-                  blueBranchA, blueBranchC, blueBranchE, blueBranchG, blueBranchI, blueBranchK)
-              : List.of(redBranchA, redBranchC, redBranchE, redBranchG, redBranchI, redBranchK);
+    private static Pose2d getNearestBranch(Pose2d p, List bluePoses, List redPoses) {
+      List<Pose2d> branchPose2ds = AllianceUtils.isBlue() ? bluePoses : redPoses;
       return p.nearest(branchPose2ds);
     }
-
-    private static Pose2d getNearestRightBranch(Pose2d p) {
-      List<Pose2d> branchPose2ds =
-          AllianceUtils.isBlue()
-              ? List.of(
-                  blueBranchB, blueBranchD, blueBranchF, blueBranchH, blueBranchJ, blueBranchL)
-              : List.of(redBranchB, redBranchD, redBranchF, redBranchH, redBranchJ, redBranchL);
-      return p.nearest(branchPose2ds);
-    }
-
-    private static Pose2d getNearestL1L(Pose2d p) {
-      List<Pose2d> reefFacesPose2ds =
-          AllianceUtils.isBlue()
-              ? List.of(
-                  lBlueReefFaceAB,
-                  lBlueReefFaceCD,
-                  lBlueReefFaceEF,
-                  lBlueReefFaceGH,
-                  lBlueReefFaceIJ,
-                  lBlueReefFaceKL)
-              : List.of(
-                  lRedReefFaceAB,
-                  lRedReefFaceCD,
-                  lRedReefFaceEF,
-                  lRedReefFaceGH,
-                  lRedReefFaceIJ,
-                  lRedReefFaceKL);
-      return p.nearest(reefFacesPose2ds);
-    }
-
-    private static Pose2d getNearestL1R(Pose2d p) {
-      List<Pose2d> reefFacesPose2ds =
-          AllianceUtils.isBlue()
-              ? List.of(
-                  rBlueReefFaceAB,
-                  rBlueReefFaceCD,
-                  rBlueReefFaceEF,
-                  rBlueReefFaceGH,
-                  rBlueReefFaceIJ,
-                  rBlueReefFaceKL)
-              : List.of(
-                  rRedReefFaceAB,
-                  rRedReefFaceCD,
-                  rRedReefFaceEF,
-                  rRedReefFaceGH,
-                  rRedReefFaceIJ,
-                  rRedReefFaceKL);
-      return p.nearest(reefFacesPose2ds);
-    }
   }
 }