Mirrored (by alliance) Piece Detection Properly

src/main/java/frc/robot/constants/AlgaePositions.java

@@ -2,6 +2,10 @@
 
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.Robot;
+import frc.robot.autochooser.FieldLocation;
+import java.util.Optional;
 
 public enum AlgaePositions {
   Algae_AB_LOW(
@@ -76,20 +80,43 @@ public enum AlgaePositions {
       true,
       new Pose3d(
           4.1615306625000, 4.5936769544649, 1.313978795, new Rotation3d(0, 0, 8.3775804095728)));
+  private final Pose3d bluePosition;
+  private final Pose3d redPosition;
   private final AlignmentPosition al1;
   private final AlignmentPosition al2;
   private final boolean high;
-  private final Pose3d position;
 
   AlgaePositions(AlignmentPosition al1, AlignmentPosition al2, boolean high, Pose3d position) {
     this.al1 = al1;
     this.al2 = al2;
     this.high = high;
-    this.position = position;
+    this.bluePosition = position;
+    double redRotationX = Math.PI + position.getRotation().getX();
+    double redRotationY = position.getRotation().getY();
+    double redRotationZ = position.getRotation().getZ();
+    Rotation3d redRotation = new Rotation3d(redRotationX, redRotationY, redRotationZ);
+
+    double redX = FieldLocation.LENGTH_OF_FIELD - bluePosition.getX();
+    double redY = FieldLocation.HEIGHT_OF_FIELD - bluePosition.getY();
+    double redZ = bluePosition.getZ();
+    this.redPosition = new Pose3d(redX, redY, redZ, redRotation);
+  }
+
+  public Pose3d getBluePosition() {
+    return bluePosition;
+  }
+
+  public Pose3d getRedPosition() {
+    return redPosition;
   }
 
   public Pose3d getPosition() {
-    return position;
+    Optional<DriverStation.Alliance> allianceColor = Robot.getAllianceColor();
+    return allianceColor
+        .map(
+            alliance ->
+                alliance == DriverStation.Alliance.Blue ? getBluePosition() : getRedPosition())
+        .orElse(new Pose3d());
   }
 
   public AlignmentPosition getAlignmentPosition1() {

src/main/java/frc/robot/constants/BranchPositions.java

@@ -2,6 +2,10 @@
 package frc.robot.constants;
 
 import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.Robot;
+import frc.robot.autochooser.FieldLocation;
+import java.util.Optional;
 
 // TODO: change all of these later to the correct values
 public enum BranchPositions {
@@ -188,16 +192,39 @@ public enum BranchPositions {
 
   private final AlignmentPosition trunk;
   private final ElevatorPosition elevatorPosition;
-  private final Pose3d position;
+  private final Pose3d bluePosition;
+  private final Pose3d redPosition;
+
+  public Pose3d getBluePosition() {
+    return bluePosition;
+  }
+
+  public Pose3d getRedPosition() {
+    return redPosition;
+  }
+
+  public Pose3d getPosition() {
+    Optional<DriverStation.Alliance> allianceColor = Robot.getAllianceColor();
+    return allianceColor
+        .map(
+            alliance ->
+                alliance == DriverStation.Alliance.Blue ? getBluePosition() : getRedPosition())
+        .orElse(new Pose3d());
+  }
 
   BranchPositions(AlignmentPosition trunk, ElevatorPosition elevatorPosition, Pose3d position) {
     this.trunk = trunk;
     this.elevatorPosition = elevatorPosition;
-    this.position = position;
-  }
+    this.bluePosition = position;
+    double redRotationX = Math.PI + position.getRotation().getX();
+    double redRotationY = position.getRotation().getY();
+    double redRotationZ = position.getRotation().getZ();
+    Rotation3d redRotation = new Rotation3d(redRotationX, redRotationY, redRotationZ);
 
-  public Pose3d getPosition() {
-    return position;
+    double redX = FieldLocation.LENGTH_OF_FIELD - bluePosition.getX();
+    double redY = FieldLocation.HEIGHT_OF_FIELD - bluePosition.getY();
+    double redZ = bluePosition.getZ();
+    this.redPosition = new Pose3d(redX, redY, redZ, redRotation);
   }
 
   public AlignmentPosition getTrunk() {

src/main/java/frc/robot/utils/GamePieceLocate.java

@@ -20,20 +20,32 @@ public record AlgaePositionMeasurement(AlgaePositions algaePosition, double conf
   private static final Comparator<BranchPositions> branchComparator =
       Comparator.comparingInt(b -> b.getElevatorLevel().getWeight());
   // Precomputed vectors for positions
-  private static final Vector<N3>[] PRECOMPUTED_BRANCH_VECS =
-      Arrays.stream(BRANCHES)
-          .map(branch -> branch.getPosition().getTranslation().toVector())
-          .toArray(Vector[]::new);
+  private static Vector<N3>[] PRECOMPUTED_BRANCH_VECS;
   ;
+  private static Vector<N3>[] PRECOMPUTED_ALGAE_VECS;
+  private static boolean vecsComputed = false;
 
-  private static final Vector<N3>[] PRECOMPUTED_ALGAE_VECS =
-      Arrays.stream(ALGAES)
-          .map(algae -> algae.getPosition().getTranslation().toVector())
-          .toArray(Vector[]::new);
+  public static void PreComputeBranchVectors() {
+    // Precomputed vectors for positions
+    PRECOMPUTED_BRANCH_VECS =
+        Arrays.stream(BRANCHES)
+            .map(branch -> branch.getPosition().getTranslation().toVector())
+            .toArray(Vector[]::new);
+    ;
+
+    PRECOMPUTED_ALGAE_VECS =
+        Arrays.stream(ALGAES)
+            .map(algae -> algae.getPosition().getTranslation().toVector())
+            .toArray(Vector[]::new);
+    vecsComputed = true;
+  }
 
   // piece pos is in DEGREES, not RD
   public static BranchPositionMeasurement findCoralBranch(
       Pose2d robotPos, double piecePosTXDeg, double piecePosTYDeg) {
+    if (!vecsComputed) {
+      PreComputeBranchVectors();
+    }
     final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT);
     final Vector<N3> cameraPosVec = cameraPos.getTranslation().toVector();
     final Matrix<N3, N3> invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix();
@@ -68,6 +80,9 @@ public static BranchPositionMeasurement findCoralBranch(
   // piece pos is in DEGREES, not RD
   public static AlgaePositionMeasurement findAlgaePos(
       Pose2d robotPos, double piecePosTXDeg, double piecePosTYDeg) {
+    if (!vecsComputed) {
+      PreComputeBranchVectors();
+    }
     final Pose3d cameraPos = new Pose3d(robotPos).transformBy(Constants.LIMELIGHT_TO_ROBOT);
     final Vector<N3> cameraPosVec = cameraPos.getTranslation().toVector();
     final Matrix<N3, N3> invCameraRotation = cameraPos.getRotation().unaryMinus().toMatrix();