Mirrors properly for alliance. Documentation in comments added. Furth…

…er changes will most likely be formatting/readability only.

src/main/java/frc/robot/Constants.java

@@ -304,19 +304,26 @@ public static final class FieldConstants {
     public static final Translation2d kFeedingSetpoint = new Translation2d(7.137, 0.872);
 
     // Reefscape constants
-    public static final Translation2d kRedReef =
-        new Translation2d(Units.inchesToMeters(177.92), Units.inchesToMeters(159.79));
-    public static final Translation2d kBlueReef =
-        new Translation2d(Units.inchesToMeters(514.66), Units.inchesToMeters(159.79));
+    public static final Translation2d kRedReefCenter =
+        new Translation2d(4.50, 4.03); // unused for now
+    public static final Translation2d kBlueReefCenter =
+        new Translation2d(13.06, 4.03); // unused for now
+    public static final Translation2d kRedCoralTip =
+        new Translation2d(3.62, 3.86); // unused for now
+    public static final Translation2d kReefDisplacementToCenter =
+        new Translation2d(4.28, 0); // mathed from above
+    public static final Translation2d kFieldCenter =
+        new Translation2d(8.78, 4.03); // mathed from above
     public static final Translation2d kBranchOffsetXY =
-        new Translation2d(Units.inchesToMeters(32.38), Units.inchesToMeters(6.814));
+        new Translation2d(0.81, 0.17); // mathed from above
+    public static final double kBranchInsetL4 =
+        0.05; // 0.81 - 0.76 // 0.76 being the x component of the L4 branch offset
     public static final double kL1Height = Units.inchesToMeters(27.0);
-    public static final double kL2Height = Units.inchesToMeters(31.72);
-    public static final double kL3Height = Units.inchesToMeters(47.59);
-    public static final double kL4Height = Units.inchesToMeters(72);
+    public static final double kL2Height = Units.inchesToMeters(32);
+    public static final double kL3Height = Units.inchesToMeters(48);
+    public static final double kL4Height = Units.inchesToMeters(73);
 
-    public static final Rotation3d kLowWristAngle =
-        new Rotation3d(0, Units.degreesToRadians(145), 0);
-    public static final Rotation3d kL4WristAngle = new Rotation3d(0, Units.degreesToRadians(90), 0);
+    public static final double kLowWristPitch = Units.degreesToRadians(145);
+    public static final double kL4WristPitch = Units.degreesToRadians(90);
   }
 }

src/main/java/frc/robot/util/ReefSelector.java

@@ -18,28 +18,42 @@ public ReefSelector() {
   }
 
   public Pose3d getPose() {
-    // do fancy maths and logic to determine offset from center to selected branch
-    Translation2d xyOffset =
-        FieldConstants.kBlueReef.plus(
-            new Translation2d(
-                    FieldConstants.kBranchOffsetXY.getX(),
-                    FieldConstants.kBranchOffsetXY.getY() * Math.pow(-1, m_position % 2))
-                .rotateBy(
-                    new Rotation2d(
-                        Units.degreesToRadians(60 * Math.floor((m_position - 0.5) / 2)))));
-
-    // do similar logic for rotation heading:
-    Rotation3d wristHeading =
-        FieldConstants.kLowWristAngle.rotateBy(
-            new Rotation3d(0, 0, Units.degreesToRadians(60 * Math.floor((m_position - 0.5) / 2))));
-    if (m_height == 4) {
-      wristHeading =
-          FieldConstants.kL4WristAngle.rotateBy(
-              new Rotation3d(
-                  0, 0, Units.degreesToRadians(60 * Math.floor((m_position - 0.5) / 2))));
-    }
 
-    // and finally, logic for the height:
+    // do fancy maths and logic to determine offset from center of reef to selected branch
+
+    // Main logic for selecting branch position relative to reef center:
+    // 1. Start with face closer to DS, right is positon 1, CW is positive increments
+    // 2. For each branch, pick the correct X inset based on height (L4 is slightly closer)
+    // 2. For each pair, pick the correct branch based on parity of m_position
+    // 3. Rotate to correct branch pair based on m_position. 1,2 -> 0, 3,4 -> 60, 5,6 -> 120, etc.
+    //    This is the job of the janky floor division
+
+    // From here, we take the reef Position and add the branch offset, then make this relative first
+    // to field center, flipping based on alliance color, then mapping to origin
+
+    Translation2d xyPosRelToCenter =
+        FieldConstants.kReefDisplacementToCenter
+            .plus(
+                new Translation2d(
+                        FieldConstants.kBranchOffsetXY.getX()
+                            - ((m_height == 4)
+                                ? FieldConstants.kBranchInsetL4
+                                : 0), // pull setpoint closer to reef for L4
+                        FieldConstants.kBranchOffsetXY.getY()
+                            * Math.pow(-1, m_position % 2)) // flip y pose for odd positions
+                    .rotateBy(
+                        new Rotation2d(
+                            Units.degreesToRadians(
+                                60
+                                    * Math.floor(
+                                        (m_position - 0.5) / 2))))) // rotate to correct branch pair
+            .rotateBy(
+                Rotation2d.fromDegrees(AllianceFlipUtil.shouldFlip() ? 180 : 0)); // flip if on red
+
+    Translation2d xyPos =
+        FieldConstants.kFieldCenter.plus(xyPosRelToCenter); // translate to origin-relative
+
+    // Pick the correct height based on m_height
     double height = 0;
     if (m_height == 1) {
       height = FieldConstants.kL1Height;
@@ -51,10 +65,26 @@ public Pose3d getPose() {
       height = FieldConstants.kL4Height;
     }
 
-    // create and return final Pose3d
-    return new Pose3d(xyOffset.getX(), xyOffset.getY(), height, wristHeading);
+    // Calculate wrist angles based on height and position
+    double wristRollRad = 0; // will be 0 for all reef positions
+    double wristPitchRad =
+        m_height != 4
+            ? FieldConstants.kLowWristPitch
+            : FieldConstants.kL4WristPitch; // adjust angle if on L4
+    double wristYawRad =
+        Units.degreesToRadians(
+            60 * Math.floor((m_position - 0.5) / 2)
+                + (AllianceFlipUtil.shouldFlip() ? 180 : 0)); // flip if on red
+
+    // Create and return final Pose3d
+    return new Pose3d(
+        xyPos.getX(),
+        xyPos.getY(),
+        height,
+        new Rotation3d(wristRollRad, wristPitchRad, wristYawRad));
   }
 
+  // Getters and setters
   public int getHeight() {
     return m_height;
   }
@@ -91,7 +121,7 @@ public void decrementPosition() {
     }
   }
 
-  public void setPosition(int position, int height) {
+  public void setPoint(int position, int height) {
     m_position = position;
     m_height = height;
   }