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