Elevator heights

src/main/java/frc/lib/util/ScoringLocation.java

@@ -15,9 +15,9 @@ public static enum CoralLocation {
      * set of height modes
      */
     public enum Height {
-        KP0("Trough", Constants.Elevator.P0), KP1("Coral 1", Constants.Elevator.P1), KP2(
-            "Algae 2", Constants.Elevator.P2), KP3("Coral 3",
-                Constants.Elevator.P3), KP4("Coral 4", Constants.Elevator.P4);
+        KP0("Trough", Constants.Elevator.P0), KP1("Coral 1", Constants.Elevator.P1), KP2("Algae 2",
+            Constants.Elevator.P2), KP3("Coral 3", Constants.Elevator.P3), KP4("Coral 4",
+                Constants.Elevator.P4), KP5("Barge", Constants.Elevator.P5);
 
         public final String displayName;
         public final Distance height;

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

@@ -339,10 +339,10 @@ public static final class Elevator {
         public static final Distance HOME = Inches.of(2);
         public static final Distance P0 = Inches.of(16.5); // Algae L2-L3
         public static final Distance P1 = Inches.of(28.1); // floor
-        public static final Distance P2 = Inches.of(31.2); // Coral l2
-        public static final Distance P3 = Inches.of(45.0); // Coral L3
-        public static final Distance P4 = Inches.of(70.0); // Coral L4
-
+        public static final Distance P2 = Inches.of(26.45); // Coral l2
+        public static final Distance P3 = Inches.of(43.6); // Coral L3
+        public static final Distance P4 = Inches.of(67.9); // Coral L4
+        public static final Distance P5 = Inches.of(70); // Barge
 
         public static final double gearRatio = 1.0;
         public static final Distance INCHES_AT_TOP = Inches.of(72.0);

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -84,6 +84,10 @@ public Command p4() {
         return moveTo(() -> Constants.Elevator.P4);
     }
 
+    public Command p5() {
+        return moveTo(() -> Constants.Elevator.P5);
+    }
+
     public boolean hightAboveP0() {
         return (inputs.position).in(Inches) >= (Constants.Elevator.P0).in(Inches) + 5;
     }