001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj2.command;
006
007import edu.wpi.first.util.sendable.SendableBuilder;
008import java.util.ArrayList;
009import java.util.List;
010
011/**
012 * A command composition that runs a list of commands in sequence.
013 *
014 * <p>The rules for command compositions apply: command instances that are passed to it cannot be
015 * added to any other composition or scheduled individually, and the composition requires all
016 * subsystems its components require.
017 *
018 * <p>This class is provided by the NewCommands VendorDep
019 */
020@SuppressWarnings("removal")
021public class SequentialCommandGroup extends CommandGroupBase {
022  private final List<Command> m_commands = new ArrayList<>();
023  private int m_currentCommandIndex = -1;
024  private boolean m_runWhenDisabled = true;
025  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
026
027  /**
028   * Creates a new SequentialCommandGroup. The given commands will be run sequentially, with the
029   * composition finishing when the last command finishes.
030   *
031   * @param commands the commands to include in this composition.
032   */
033  public SequentialCommandGroup(Command... commands) {
034    addCommands(commands);
035  }
036
037  @Override
038  public final void addCommands(Command... commands) {
039    if (m_currentCommandIndex != -1) {
040      throw new IllegalStateException(
041          "Commands cannot be added to a composition while it's running");
042    }
043
044    CommandScheduler.getInstance().registerComposedCommands(commands);
045
046    for (Command command : commands) {
047      m_commands.add(command);
048      m_requirements.addAll(command.getRequirements());
049      m_runWhenDisabled &= command.runsWhenDisabled();
050      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
051        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
052      }
053    }
054  }
055
056  @Override
057  public final void initialize() {
058    m_currentCommandIndex = 0;
059
060    if (!m_commands.isEmpty()) {
061      m_commands.get(0).initialize();
062    }
063  }
064
065  @Override
066  public final void execute() {
067    if (m_commands.isEmpty()) {
068      return;
069    }
070
071    Command currentCommand = m_commands.get(m_currentCommandIndex);
072
073    currentCommand.execute();
074    if (currentCommand.isFinished()) {
075      currentCommand.end(false);
076      m_currentCommandIndex++;
077      if (m_currentCommandIndex < m_commands.size()) {
078        m_commands.get(m_currentCommandIndex).initialize();
079      }
080    }
081  }
082
083  @Override
084  public final void end(boolean interrupted) {
085    if (interrupted
086        && !m_commands.isEmpty()
087        && m_currentCommandIndex > -1
088        && m_currentCommandIndex < m_commands.size()) {
089      m_commands.get(m_currentCommandIndex).end(true);
090    }
091    m_currentCommandIndex = -1;
092  }
093
094  @Override
095  public final boolean isFinished() {
096    return m_currentCommandIndex == m_commands.size();
097  }
098
099  @Override
100  public boolean runsWhenDisabled() {
101    return m_runWhenDisabled;
102  }
103
104  @Override
105  public InterruptionBehavior getInterruptionBehavior() {
106    return m_interruptBehavior;
107  }
108
109  @Override
110  public void initSendable(SendableBuilder builder) {
111    super.initSendable(builder);
112
113    builder.addIntegerProperty("index", () -> m_currentCommandIndex, null);
114  }
115}