Optimize copying m_scheduledCommands

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

@@ -67,6 +67,9 @@ public static synchronized CommandScheduler getInstance() {
 
  // A set of the currently-running commands.
  private final Set<Command> m_scheduledCommands = new LinkedHashSet<>();
+ // A copy of the currently-running commands, used for iteration stored on class for caching
+ // purposes.
+ private Command[] m_scheduledCommandsCopy = new Command[0];
 
  // A map from required subsystems to their requiring commands. Also used as a set of the
  // currently-required subsystems.
@@ -263,9 +266,13 @@ public void run() {
 
  boolean isDisabled = RobotState.isDisabled();
  // Run scheduled commands, remove finished commands.
- for (Command command : Set.copyOf(m_scheduledCommands)) {
- if (!isScheduled(command)) {
- continue; // skip as the normal scheduledCommands was modified and that command was canceled
+ m_scheduledCommandsCopy = m_scheduledCommands.toArray(m_scheduledCommandsCopy);
+ for (Command command : m_scheduledCommandsCopy) {
+ boolean isCanceledBeforeLoop = (command == null);
+ boolean isCanceledInLoop = !isScheduled(command);
+
+ if (isCanceledBeforeLoop || isCanceledInLoop) {
+ continue;
  }
 
  if (isDisabled && !command.runsWhenDisabled()) {