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 org.wpilib.command2;
006
007import java.util.ArrayList;
008import java.util.List;
009import org.wpilib.util.sendable.SendableBuilder;
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 Commands v2 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  @SuppressWarnings("this-escape")
033  public SequentialCommandGroup(Command... commands) {
034    addCommands(commands);
035  }
036
037  /**
038   * Adds the given commands to the group.
039   *
040   * @param commands Commands to add, in order of execution.
041   */
042  public final void addCommands(Command... commands) {
043    if (m_currentCommandIndex != -1) {
044      throw new IllegalStateException(
045          "Commands cannot be added to a composition while it's running");
046    }
047
048    CommandScheduler.getInstance().registerComposedCommands(commands);
049
050    for (Command command : commands) {
051      m_commands.add(command);
052      addRequirements(command.getRequirements());
053      m_runWhenDisabled &= command.runsWhenDisabled();
054      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
055        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
056      }
057    }
058  }
059
060  @Override
061  public final void initialize() {
062    m_currentCommandIndex = 0;
063
064    if (!m_commands.isEmpty()) {
065      m_commands.get(0).initialize();
066    }
067  }
068
069  @Override
070  public final void execute() {
071    if (m_commands.isEmpty()) {
072      return;
073    }
074
075    Command currentCommand = m_commands.get(m_currentCommandIndex);
076
077    currentCommand.execute();
078    if (currentCommand.isFinished()) {
079      currentCommand.end(false);
080      m_currentCommandIndex++;
081      if (m_currentCommandIndex < m_commands.size()) {
082        m_commands.get(m_currentCommandIndex).initialize();
083      }
084    }
085  }
086
087  @Override
088  public final void end(boolean interrupted) {
089    if (interrupted
090        && !m_commands.isEmpty()
091        && m_currentCommandIndex > -1
092        && m_currentCommandIndex < m_commands.size()) {
093      m_commands.get(m_currentCommandIndex).end(true);
094    }
095    m_currentCommandIndex = -1;
096  }
097
098  @Override
099  public final boolean isFinished() {
100    return m_currentCommandIndex == m_commands.size();
101  }
102
103  @Override
104  public boolean runsWhenDisabled() {
105    return m_runWhenDisabled;
106  }
107
108  @Override
109  public InterruptionBehavior getInterruptionBehavior() {
110    return m_interruptBehavior;
111  }
112
113  @Override
114  public void initSendable(SendableBuilder builder) {
115    super.initSendable(builder);
116
117    builder.addIntegerProperty("index", () -> m_currentCommandIndex, null);
118  }
119}