Feature/pole height switcher

docs/README.md

@@ -13,3 +13,9 @@
 [Autos](/docs/autos/CalculatedFieldPos.md)
 
 [WPILib rotations](/docs/rotations/rotations.md)
+
+[CANID](/docs/CANID/CANID.md)
+
+[Controller Mappings](/docs/ControllerMapping/controller-mapping.md)
+
+[Reef Standard Names/Id Given](/docs/reefFaceStandard/reef.md)

docs/reefFaceStandard/reef.md

@@ -0,0 +1 @@
+![reef](/docs/reefFaceStandard/reef.png)

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

@@ -426,6 +426,9 @@ public void periodic() {
                         .getPose("Main")
                         .getTranslation()
                         .getDistance(getTarget().getTranslation()));
+                Logger.recordOutput("Pole", getPoleID());
+                Logger.recordOutput("Coral Offset", getCoralOffset(getPoleID()));
+
                 Logger.recordOutput("target", getTarget());
               }
             });
@@ -444,6 +447,39 @@ public void periodic() {
             driveController);
   }
 
+  public int getPoleID() {
+    Pose2d pose =
+        DistanceManager.getNearestPosition(
+            RobotOdometry.instance.getPose("Main"),
+            AllianceManager.chooseFromAlliance(
+                FieldConstants.reefPositionsBlue, FieldConstants.reefPositionsRed));
+    int poleID = 0;
+    for (int i = 0;
+        i
+            < AllianceManager.chooseFromAlliance(
+                FieldConstants.reefPositionsBlue.length, FieldConstants.reefPositionsRed.length);
+        i++) {
+      if (pose.getTranslation()
+              .getDistance(
+                  AllianceManager.chooseFromAlliance(
+                          FieldConstants.reefPositionsBlue[i], FieldConstants.reefPositionsRed[i])
+                      .getTranslation())
+          < 0.2) {
+        poleID = i;
+      }
+    }
+
+    poleID = (poleID * 2) + (coralPreset.isRight() ? 0 : 1);
+
+    return poleID;
+  }
+
+  public double getCoralOffset(int poleID) {
+    return AllianceManager.chooseFromAlliance(
+        FieldConstants.reefPoleRegularHeight - FieldConstants.reefPoleHeightBlue[poleID],
+        FieldConstants.reefPoleRegularHeight - FieldConstants.reefPoleHeightRed[poleID]);
+  }
+
   public Pose2d coralAdjust(Pose2d pose, Supplier<CoralPreset> preset) {
     if (algaeMode) {
       return pose;
@@ -915,9 +951,10 @@ public Command setupAutoPlace(Supplier<CoralPreset> coralPreset) {
               (new InstantCommand(
                           () -> {
                             presetActive =
-                                algaeMode
-                                    ? coralPreset.get().getLiftAlgae()
-                                    : coralPreset.get().getLift();
+                                (algaeMode
+                                        ? coralPreset.get().getLiftAlgae()
+                                        : coralPreset.get().getLift())
+                                    + (coralPreset.get().isL4() ? getCoralOffset(getPoleID()) : 0);
                             gantryPresetActive = coralPreset.get();
                           })
                       .andThen(

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

@@ -34,24 +34,25 @@ public class FieldConstants {
   public static final int kSouthWest = 4;
   public static final int kNorthWest = 5;
 
+  public static final double reefPoleRegularHeight = 0.568; // TODO Switch from fake units to meters
   public static final Pose2d[] reefPositionsRed =
       new Pose2d[] {
-        new Pose2d(12.227305999999999, 4.0259, Rotation2d.fromDegrees(360)),
-        new Pose2d(12.643358, 4.745482, Rotation2d.fromDegrees(300)),
-        new Pose2d(13.474446, 4.745482, Rotation2d.fromDegrees(240)),
-        new Pose2d(13.890498, 4.0259, Rotation2d.fromDegrees(180)),
-        new Pose2d(13.474446, 3.3063179999999996, Rotation2d.fromDegrees(120)),
-        new Pose2d(12.643358, 3.3063179999999996, Rotation2d.fromDegrees(60)),
+        new Pose2d(13.890498, 4.0259, Rotation2d.fromDegrees(180)), // id 7
+        new Pose2d(13.474446, 3.3063179999999996, Rotation2d.fromDegrees(120)), // id 6
+        new Pose2d(12.643358, 3.3063179999999996, Rotation2d.fromDegrees(60)), // id 11
+        new Pose2d(12.227305999999999, 4.0259, Rotation2d.fromDegrees(360)), // id 10
+        new Pose2d(12.643358, 4.745482, Rotation2d.fromDegrees(300)), // id 9
+        new Pose2d(13.474446, 4.745482, Rotation2d.fromDegrees(240)), // id 8
       };
 
   public static final Pose2d[] reefPositionsBlue =
       new Pose2d[] {
-        new Pose2d(5.321046, 4.0259, Rotation2d.fromDegrees(180)),
-        new Pose2d(4.904739999999999, 3.3063179999999996, Rotation2d.fromDegrees(480)),
-        new Pose2d(4.073905999999999, 3.3063179999999996, Rotation2d.fromDegrees(420)),
-        new Pose2d(3.6576, 4.0259, Rotation2d.fromDegrees(360)),
-        new Pose2d(4.073905999999999, 4.745482, Rotation2d.fromDegrees(300)),
-        new Pose2d(4.904739999999999, 4.745482, Rotation2d.fromDegrees(240)),
+        new Pose2d(3.6576, 4.0259, Rotation2d.fromDegrees(360)), // id 18
+        new Pose2d(4.073905999999999, 4.745482, Rotation2d.fromDegrees(300)), // id 19
+        new Pose2d(4.904739999999999, 4.745482, Rotation2d.fromDegrees(240)), // id 20
+        new Pose2d(5.321046, 4.0259, Rotation2d.fromDegrees(180)), // id 21
+        new Pose2d(4.904739999999999, 3.3063179999999996, Rotation2d.fromDegrees(480)), // id 22
+        new Pose2d(4.073905999999999, 3.3063179999999996, Rotation2d.fromDegrees(420)), // id 17
       };
 
   public static final Translation2d reefCenterPosRed = new Translation2d(13.07, 4);
@@ -75,4 +76,11 @@ public class FieldConstants {
         new Pose2d(16.697198, 0.65532, Rotation2d.fromDegrees(126)), // 1
         new Pose2d(16.697198, 7.3964799999999995, Rotation2d.fromDegrees(234)) // 2
       };
+  // left is +1 id
+  public static final double[] reefPoleHeightBlue = {
+    1.825, 1.821, 1.816, 1.812, 1.806, 1.811, 1.820, 1.820, 1.825, 1.825, 1.822, 1.825,
+  }; // TODO Convert the lift from FakeUnit to Meters
+  public static final double[] reefPoleHeightRed = {
+    1.825, 1.821, 1.816, 1.812, 1.806, 1.811, 1.820, 1.820, 1.825, 1.825, 1.822, 1.825,
+  }; /// TODO Convert the lift from FakeUnit to Meters
 }

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

@@ -317,6 +317,10 @@ public double getLiftAlgae() {
         return liftAlgae;
       }
 
+      public boolean isL4() {
+        return this == LeftL4 || this == RightL4;
+      }
+
       public double getGantry(boolean dsSide) {
         switch (gantrySetpoint) {
           case LEFT: