Climb Command (L1/L2/L3 staging)

Disclaimer: WIP and untested on 2026 hardware; verify heights, limits, and safety interlocks before real climbs. Custom code built on WPILib APIs (not official WPILib sample).

Purpose: Provide a reliable climb routine that can be scoped to L1/L2/L3 with clear aborts and END GAME lockout.

What it does

• Lockout until END GAME window.
• Staged climb: extend → hook detect → lift → hold.
• Supports targeting L1/L2/L3 (heights from manual: 27/45/63 in).
• Safety: timeouts per stage, current limit, manual abort.

Code (Java/WPILib Command)

public class ClimbCommand extends SequentialCommandGroup {
  public ClimbCommand(ClimbSubsystem climb, Supplier<Boolean> endgame, Level target, DoubleSupplier abortButton) {
    addCommands(
      new WaitUntilCommand(endgame::get), // lockout until END GAME
      new ConditionalCommand(
        new InstantCommand(), // abort if button held
        sequenceForLevel(climb, target, abortButton),
        () -> abortButton.getAsDouble() > 0.5
      )
    );
  }

  private Command sequenceForLevel(ClimbSubsystem climb, Level target, DoubleSupplier abort) {
    double height = switch (target) {
      case L1 -> 27.0;
      case L2 -> 45.0;
      case L3 -> 63.0;
    };
    return new SequentialCommandGroup(
      new RunCommand(() -> climb.extendTo(height), climb).withTimeout(3.0),
      new WaitUntilCommand(climb::hooked).withTimeout(1.0),
      new RunCommand(climb::lift, climb).withTimeout(3.0),
      new InstantCommand(climb::hold, climb)
    ).until(() -> abort.getAsDouble() > 0.5); // abort anytime
  }

  public enum Level { L1, L2, L3 }
}

ClimbSubsystem sketch

public class ClimbSubsystem extends SubsystemBase {
  private final TalonFX climbMotor = new TalonFX(20);
  private final DutyCycleEncoder encoder = new DutyCycleEncoder(0);
  private boolean hooked = false;

  public void extendTo(double inches) {
    double ticks = inchesToTicks(inches);
    climbMotor.set(ControlMode.Position, ticks);
  }
  public void lift() { climbMotor.set(ControlMode.PercentOutput, -0.6); }
  public void hold() { climbMotor.set(ControlMode.PercentOutput, -0.1); }
  public boolean hooked() { return hooked; }
  private double inchesToTicks(double inches) { return inches * 100.0; } // calibrate

  @Override
  public void periodic() {
    // Hook detection: beam/limit or current spike
    hooked = climbMotor.getStatorCurrent() > 25.0;
  }
}

Parameters to tune

• Heights per level: 27/45/63 in; convert to ticks based on rig.
• Current limits and hold power.
• Timeouts per stage.
• Hook detection: prefer limit switch/beam over current spike.

How to test

• Dry run off-robot with encoder sim; confirm no movement before END GAME.
• On robot: test L1 first with a strap, verify timeouts and abort; then L2; only attempt L3 after L2 is 90%+ repeatable.
• Log current and position during each stage.

Pitfalls

• Never allow climb before END GAME (lockout guard).
• Add hard limits to avoid over-travel.
• Ensure abort works even if command is mid-stage.