Shift-Aware Path Switch

Disclaimer: WIP and untested; adapt to your path library. Custom code built on WPILib APIs.

Concept: While running a scoring path, monitor hub state; if it flips to inactive, cancel and run a short stash/hoard path, then resume scoring when active or skip to next action.

Code (Java, PathPlanner example)

public class ShiftAwarePath extends Command {
  private final Command scoringPath;
  private final Command stashPath;
  private final Supplier<HubShiftTimer.HubState> hub;

  public ShiftAwarePath(Command scoringPath, Command stashPath, Supplier<HubShiftTimer.HubState> hub) {
    this.scoringPath = scoringPath;
    this.stashPath = stashPath;
    this.hub = hub;
  }

  @Override
  public void initialize() {
    scoringPath.initialize();
  }

  @Override
  public void execute() {
    if (hub.get() == HubShiftTimer.HubState.INACTIVE) {
      scoringPath.end(true);
      stashPath.schedule();
    } else {
      scoringPath.execute();
    }
  }

  @Override
  public boolean isFinished() {
    return scoringPath.isFinished() || stashPath.isScheduled();
  }

  @Override
  public void end(boolean interrupted) {
    scoringPath.end(interrupted);
  }
}

Pitfalls: ensure stash path is short/safe; avoid re-scheduling repeatedly; consider a state machine instead of raw command wrapping.