auto climb code

Author: aahFancyGun

src/main/java/frc/robot/HeadHoncho.java

@@ -48,6 +48,12 @@ public void initialize() {
 
       @Override
       public SystemState nextState() {
+
+        if(s_autoClimb && DriverStation.isFMSAttached() && DriverStation.isTeleopEnabled() && DriverStation.getMatchTime() <= 22.0){
+          s_autoClimb = false;
+          return MOUNT;
+        }
+
         if (s_intakeButton.getAsBoolean() && END_EFFECTOR_SUBSYSTEM.isEmpty() && LIFT_SUBSYSTEM.isAtState(TargetLiftStates.STOW)) {
           return INTAKE;
         }
@@ -367,6 +373,7 @@ public void end(boolean interrupted) {
   private static BooleanSupplier s_climbButton;
   private static Boolean s_lastClimbBoolean = false;
   private static Boolean s_climbButtonRising = false;
+  private static Boolean s_autoClimb = true;
 
   public HeadHoncho(
       DriveSubsystem driveSubsystem,