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.HashMap;
009import java.util.Map;
010
011/**
012 * A command composition that runs a set of commands in parallel, ending when the last command ends.
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 ParallelCommandGroup extends Command {
021  // maps commands in this composition to whether they are still running
022  private final Map<Command, Boolean> m_commands = new HashMap<>();
023  private boolean m_runWhenDisabled = true;
024  private InterruptionBehavior m_interruptBehavior = InterruptionBehavior.kCancelIncoming;
025
026  /**
027   * Creates a new ParallelCommandGroup. The given commands will be executed simultaneously. The
028   * command composition will finish when the last command finishes. If the composition is
029   * interrupted, only the commands that are still running will be interrupted.
030   *
031   * @param commands the commands to include in this composition.
032   */
033  public ParallelCommandGroup(Command... commands) {
034    addCommands(commands);
035  }
036
037  /**
038   * Adds the given commands to the group.
039   *
040   * @param commands Commands to add to the group.
041   */
042  public final void addCommands(Command... commands) {
043    if (m_commands.containsValue(true)) {
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      if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
052        throw new IllegalArgumentException(
053            "Multiple commands in a parallel composition cannot require the same subsystems");
054      }
055      m_commands.put(command, false);
056      m_requirements.addAll(command.getRequirements());
057      m_runWhenDisabled &= command.runsWhenDisabled();
058      if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
059        m_interruptBehavior = InterruptionBehavior.kCancelSelf;
060      }
061    }
062  }
063
064  @Override
065  public final void initialize() {
066    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
067      commandRunning.getKey().initialize();
068      commandRunning.setValue(true);
069    }
070  }
071
072  @Override
073  public final void execute() {
074    for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
075      if (!commandRunning.getValue()) {
076        continue;
077      }
078      commandRunning.getKey().execute();
079      if (commandRunning.getKey().isFinished()) {
080        commandRunning.getKey().end(false);
081        commandRunning.setValue(false);
082      }
083    }
084  }
085
086  @Override
087  public final void end(boolean interrupted) {
088    if (interrupted) {
089      for (Map.Entry<Command, Boolean> commandRunning : m_commands.entrySet()) {
090        if (commandRunning.getValue()) {
091          commandRunning.getKey().end(true);
092        }
093      }
094    }
095  }
096
097  @Override
098  public final boolean isFinished() {
099    return !m_commands.containsValue(true);
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}