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()) {