Safe Shot Sequencer

Disclaimer: WIP and untested; tune timings and thresholds on your robot. Custom code built on WPILib APIs (not official WPILib sample).

Code (Java/WPILib)

public class SafeShot extends SequentialCommandGroup {
  public SafeShot(ShooterSubsystem shooter, IndexerSubsystem indexer,
                  Supplier<HubShiftTimer.HubState> hub, double targetRpm) {
    addCommands(
      new InstantCommand(() -> shooter.setTargetRpm(targetRpm)),
      // Optional anti-jam: short reverse then stop
      new InstantCommand(indexer::reverse).withTimeout(0.2),
      new InstantCommand(indexer::stop),
      new WaitUntilCommand(() -> shooter.ready(100) && hub.get() == HubShiftTimer.HubState.ACTIVE),
      new RunCommand(indexer::feedOut, indexer).withTimeout(1.0),
      new InstantCommand(() -> shooter.setTargetRpm(0)),
      new InstantCommand(indexer::stop)
    );
  }
}