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 */
020public class SequentialCommandGroup extends Command {
021  private final List<Command> m_commands = new ArrayList<>();
022  private int m_currentCommandIndex = -1;
023  private boolean m_runWhenDisabled = true;
024  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
025
026  /**
027   * Creates a new SequentialCommandGroup. The given commands will be run sequentially, with the
028   * composition finishing when the last command finishes.
029   *
030   * @param commands the commands to include in this composition.
031   */
032  public SequentialCommandGroup(Command... commands) {
033    addCommands(commands);
034  }
035
036  /**
037   * Adds the given commands to the group.
038   *
039   * @param commands Commands to add, in order of execution.
040   */
041  public final void addCommands(Command... commands) {
042    if (m_currentCommandIndex != -1) {
043      throw new IllegalStateException(
044          "Commands cannot be added to a composition while it's running");
045    }
046
047    CommandScheduler.getInstance().registerComposedCommands(commands);
048
049    for (Command command : commands) {
050      m_commands.add(command);
051      m_requirements.addAll(command.getRequirements());
052      m_runWhenDisabled &= command.runsWhenDisabled();
053      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
054        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
055      }
056    }
057  }
058
059  @Override
060  public final void initialize() {
061    m_currentCommandIndex = 0;
062
063    if (!m_commands.isEmpty()) {
064      m_commands.get(0).initialize();
065    }
066  }
067
068  @Override
069  public final void execute() {
070    if (m_commands.isEmpty()) {
071      return;
072    }
073
074    Command currentCommand = m_commands.get(m_currentCommandIndex);
075
076    currentCommand.execute();
077    if (currentCommand.isFinished()) {
078      currentCommand.end(false);
079      m_currentCommandIndex++;
080      if (m_currentCommandIndex < m_commands.size()) {
081        m_commands.get(m_currentCommandIndex).initialize();
082      }
083    }
084  }
085
086  @Override
087  public final void end(boolean interrupted) {
088    if (interrupted
089        && !m_commands.isEmpty()
090        && m_currentCommandIndex > -1
091        && m_currentCommandIndex < m_commands.size()) {
092      m_commands.get(m_currentCommandIndex).end(true);
093    }
094    m_currentCommandIndex = -1;
095  }
096
097  @Override
098  public final boolean isFinished() {
099    return m_currentCommandIndex == m_commands.size();
100  }
101
102  @Override
103  public boolean runsWhenDisabled() {
104    return m_runWhenDisabled;
105  }
106
107  @Override
108  public InterruptionBehavior getInterruptionBehavior() {
109    return m_interruptBehavior;
110  }
111
112  @Override
113  public void initSendable(SendableBuilder builder) {
114    super.initSendable(builder);
115
116    builder.addIntegerProperty("index", () -> m_currentCommandIndex, null);
117  }
118}