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 java.util.Collections;
008import java.util.HashSet;
009import java.util.Set;
010
011/**
012 * A composition that runs a set of commands in parallel, ending when any one of the commands ends
013 * and interrupting all the others.
014 *
015 * <p>The rules for command compositions apply: command instances that are passed to it cannot be
016 * added to any other composition or scheduled individually, and the composition requires all
017 * subsystems its components require.
018 *
019 * <p>This class is provided by the NewCommands VendorDep
020 */
021public class ParallelRaceGroup extends Command {
022  private final Set<Command> m_commands = new HashSet<>();
023  private boolean m_runWhenDisabled = true;
024  private boolean m_finished = true;
025  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
026
027  /**
028   * Creates a new ParallelCommandRace. The given commands will be executed simultaneously, and will
029   * "race to the finish" - the first command to finish ends the entire command, with all other
030   * commands being interrupted.
031   *
032   * @param commands the commands to include in this composition.
033   */
034  public ParallelRaceGroup(Command... commands) {
035    addCommands(commands);
036  }
037
038  /**
039   * Adds the given commands to the group.
040   *
041   * @param commands Commands to add to the group.
042   */
043  public final void addCommands(Command... commands) {
044    if (!m_finished) {
045      throw new IllegalStateException(
046          "Commands cannot be added to a composition while it's running!");
047    }
048
049    CommandScheduler.getInstance().registerComposedCommands(commands);
050
051    for (Command command : commands) {
052      if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
053        throw new IllegalArgumentException(
054            "Multiple commands in a parallel composition cannot require the same subsystems");
055      }
056      m_commands.add(command);
057      m_requirements.addAll(command.getRequirements());
058      m_runWhenDisabled &= command.runsWhenDisabled();
059      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
060        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
061      }
062    }
063  }
064
065  @Override
066  public final void initialize() {
067    m_finished = false;
068    for (Command command : m_commands) {
069      command.initialize();
070    }
071  }
072
073  @Override
074  public final void execute() {
075    for (Command command : m_commands) {
076      command.execute();
077      if (command.isFinished()) {
078        m_finished = true;
079      }
080    }
081  }
082
083  @Override
084  public final void end(boolean interrupted) {
085    for (Command command : m_commands) {
086      command.end(!command.isFinished());
087    }
088  }
089
090  @Override
091  public final boolean isFinished() {
092    return m_finished;
093  }
094
095  @Override
096  public boolean runsWhenDisabled() {
097    return m_runWhenDisabled;
098  }
099
100  @Override
101  public InterruptionBehavior getInterruptionBehavior() {
102    return m_interruptBehavior;
103  }
104}