Merge branch 'main' into update-Intake

Author: BrodyKarr

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

@@ -301,10 +301,10 @@ public static boolean isRedAlliance() {
 
   public static class constElevator {
 
-    public static final Distance CORAL_L1_HEIGHT = Units.Inches.of(0);
-    public static final Distance CORAL_L2_HEIGHT = Units.Inches.of(0);
-    public static final Distance CORAL_L3_HEIGHT = Units.Inches.of(0);
-    public static final Distance CORAL_L4_HEIGHT = Units.Inches.of(0);
+    public static final Distance CORAL_L1_HEIGHT = Units.Inches.of(1);
+    public static final Distance CORAL_L2_HEIGHT = Units.Inches.of(2);
+    public static final Distance CORAL_L3_HEIGHT = Units.Inches.of(3);
+    public static final Distance CORAL_L4_HEIGHT = Units.Inches.of(4);
     public static final Distance ALGAE_NET_HEIGHT = Units.Inches.of(0);
     public static final Distance DEADZONE_DISTANCE = Units.Inches.of(0);
 

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

@@ -205,7 +205,8 @@ private void configOperatorBindings() {
 
     conOperator.btn_RightStick
         .onTrue(TRY_PREP_CORAL_ZERO)
-        .onTrue(TRY_PREP_CORAL_ZERO_WITH_ALGAE);
+        .onTrue(TRY_PREP_CORAL_ZERO_WITH_ALGAE)
+        .onTrue(TRY_PREP_ALGAE_ZERO);
 
     conOperator.btn_North
         .onTrue(TRY_PREP_ALGAE_NET)

src/main/java/frc/robot/subsystems/StateMachine.java

@@ -315,6 +315,7 @@ public Command tryState(RobotState desiredState) {
           case INTAKE_ALGAE_GROUND:
           case SCORING_CORAL_WITH_ALGAE:
           case INTAKE_CORAL_GROUND_WITH_ALGAE:
+          case INTAKE_CORAL_STATION_WITH_ALGAE:
             return new HasAlgae(subStateMachine);
         }
         break;
@@ -483,8 +484,9 @@ public Command tryState(RobotState desiredState) {
         switch (currentRobotState) {
           case HAS_ALGAE:
           case INTAKE_CORAL_GROUND_WITH_ALGAE:
-
+            return new IntakeCoralStationWithAlgae(subStateMachine);
         }
+        break;
     }
     return Commands.print("ITS SO OVER D: Invalid State Provided, Blame Eli. Attempted to go to: "
         + desiredState.toString() + " while at " + currentRobotState.toString());