[commands] Update requirements consistently

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java

@@ -13,6 +13,7 @@
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.Collection;
 import java.util.HashSet;
 import java.util.Set;
 import java.util.function.BooleanSupplier;
@@ -96,6 +97,19 @@ public final void addRequirements(Subsystem... requirements) {
     }
   }
 
+  /**
+   * Adds the specified subsystems to the requirements of the command. The scheduler will prevent
+   * two commands that require the same subsystem from being scheduled simultaneously.
+   *
+   * <p>Note that the scheduler determines the requirements of a command when it is scheduled, so
+   * this method should normally be called from the command's constructor.
+   *
+   * @param requirements the requirements to add
+   */
+  public final void addRequirements(Collection<Subsystem> requirements) {
+    m_requirements.addAll(requirements);
+  }
+
   /**
    * Gets the name of this Command.
    *

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java

@@ -39,8 +39,8 @@ public ConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condi
 
     CommandScheduler.getInstance().registerComposedCommands(onTrue, onFalse);
 
-    m_requirements.addAll(m_onTrue.getRequirements());
-    m_requirements.addAll(m_onFalse.getRequirements());
+    addRequirements(m_onTrue.getRequirements());
+    addRequirements(m_onFalse.getRequirements());
   }
 
   @Override

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java

@@ -7,7 +7,6 @@
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.math.controller.PIDController;
-import java.util.Set;
 import java.util.function.DoubleConsumer;
 import java.util.function.DoubleSupplier;
 
@@ -55,7 +54,7 @@ public PIDCommand(
     m_useOutput = useOutput;
     m_measurement = measurementSource;
     m_setpoint = setpointSource;
-    m_requirements.addAll(Set.of(requirements));
+    addRequirements(requirements);
   }
 
   /**

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java

@@ -55,7 +55,7 @@ public final void addCommands(Command... commands) {
             "Multiple commands in a parallel composition cannot require the same subsystems");
       }
       m_commands.put(command, false);
-      m_requirements.addAll(command.getRequirements());
+      addRequirements(command.getRequirements());
       m_runWhenDisabled &= command.runsWhenDisabled();
       if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
         m_interruptBehavior = InterruptionBehavior.kCancelSelf;

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java

@@ -85,7 +85,7 @@ public final void addCommands(Command... commands) {
             "Multiple commands in a parallel group cannot require the same subsystems");
       }
       m_commands.put(command, false);
-      m_requirements.addAll(command.getRequirements());
+      addRequirements(command.getRequirements());
       m_runWhenDisabled &= command.runsWhenDisabled();
       if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
         m_interruptBehavior = InterruptionBehavior.kCancelSelf;

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java

@@ -56,7 +56,7 @@ public final void addCommands(Command... commands) {
             "Multiple commands in a parallel composition cannot require the same subsystems");
       }
       m_commands.add(command);
-      m_requirements.addAll(command.getRequirements());
+      addRequirements(command.getRequirements());
       m_runWhenDisabled &= command.runsWhenDisabled();
       if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
         m_interruptBehavior = InterruptionBehavior.kCancelSelf;

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java

@@ -8,7 +8,6 @@
 import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.math.controller.ProfiledPIDController;
-import java.util.Set;
 import java.util.function.BiConsumer;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
@@ -58,7 +57,7 @@ public ProfiledPIDCommand(
     m_useOutput = useOutput;
     m_measurement = measurementSource;
     m_goal = goalSource;
-    m_requirements.addAll(Set.of(requirements));
+    addRequirements(requirements);
   }
 
   /**
@@ -86,7 +85,7 @@ public ProfiledPIDCommand(
     m_useOutput = useOutput;
     m_measurement = measurementSource;
     m_goal = () -> new State(goalSource.getAsDouble(), 0);
-    m_requirements.addAll(Set.of(requirements));
+    addRequirements(requirements);
   }
 
   /**

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java

@@ -33,7 +33,7 @@ public class RepeatCommand extends Command {
   public RepeatCommand(Command command) {
     m_command = requireNonNullParam(command, "command", "RepeatCommand");
     CommandScheduler.getInstance().registerComposedCommands(command);
-    m_requirements.addAll(command.getRequirements());
+    addRequirements(command.getRequirements());
     setName("Repeat(" + command.getName() + ")");
   }
 

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java

@@ -47,7 +47,7 @@ public SelectCommand(Map<K, Command> commands, Supplier<? extends K> selector) {
         .registerComposedCommands(commands.values().toArray(new Command[] {}));
 
     for (Command command : m_commands.values()) {
-      m_requirements.addAll(command.getRequirements());
+      addRequirements(command.getRequirements());
       m_runsWhenDisabled &= command.runsWhenDisabled();
       if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
         m_interruptBehavior = InterruptionBehavior.kCancelSelf;

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java

@@ -49,7 +49,7 @@ public final void addCommands(Command... commands) {
 
     for (Command command : commands) {
       m_commands.add(command);
-      m_requirements.addAll(command.getRequirements());
+      addRequirements(command.getRequirements());
       m_runWhenDisabled &= command.runsWhenDisabled();
       if (command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
         m_interruptBehavior = InterruptionBehavior.kCancelSelf;