diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java index 255291b933e..1364c1f3bd1 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java +++ b/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. + * + *

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 requirements) { + m_requirements.addAll(requirements); + } + /** * Gets the name of this Command. * diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java index 42db3f0b996..15c8674da7b 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java +++ b/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 diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java index 26627fa30ff..fed674413b6 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java +++ b/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); } /** diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java index 6ea58698c60..cb5dcf0c0e2 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java +++ b/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; diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java index eb576d75872..fae0542322a 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java +++ b/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; diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java index 39fe6114b41..538ab11d5d2 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java +++ b/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; diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java index 4e82811739b..39bdf246c18 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java +++ b/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); } /** diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java index 70ff63c6103..6c64b4f2e4d 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RepeatCommand.java +++ b/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() + ")"); } diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java index 28d06eb0851..b888e98eb5f 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java @@ -47,7 +47,7 @@ public SelectCommand(Map commands, Supplier 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; diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java index 87805972c38..905b8a6bbd3 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java +++ b/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;