001// Copyright (c) Choreo contributors
002
003package choreo.auto;
004
005import org.wpilib.command2.Command;
006import org.wpilib.command2.CommandScheduler;
007import org.wpilib.command2.Commands;
008import org.wpilib.opmode.OpMode;
009
010/** An OpMode that runs a routine selected from an AutoChooser. */
011public class AutoChooserOpMode implements OpMode {
012  private final AutoChooser autoChooser;
013  private Command autonomousCommand = Commands.none();
014
015  /**
016   * Creates a new AutoChooserOpMode.
017   *
018   * @param autoChooser the AutoChooser to use for selecting the autonomous routine to run.
019   */
020  public AutoChooserOpMode(AutoChooser autoChooser) {
021    this.autoChooser = autoChooser;
022  }
023
024  @Override
025  public void start() {
026    autonomousCommand = autoChooser.selectedCommand();
027
028    if (autonomousCommand != null) {
029      CommandScheduler.getInstance().schedule(autonomousCommand);
030    }
031  }
032
033  @Override
034  public void end() {
035    if (autonomousCommand != null) {
036      autonomousCommand.cancel();
037    }
038  }
039}