[commands] Test no-op behavior of scheduling a scheduled command (#4806)

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

@@ -217,8 +217,8 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
     // Do nothing if the scheduler is disabled, the robot is disabled and the command doesn't
     // run when disabled, or the command is already scheduled.
     if (m_disabled
-        || RobotState.isDisabled() && !command.runsWhenDisabled()
-        || isScheduled(command)) {
+        || isScheduled(command)
+        || RobotState.isDisabled() && !command.runsWhenDisabled()) {
       return;
     }
 

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp

@@ -119,9 +119,8 @@ void CommandScheduler::Schedule(Command* command) {
 
   RequireUngrouped(command);
 
-  if (m_impl->disabled ||
-      (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
-      m_impl->scheduledCommands.contains(command)) {
+  if (m_impl->disabled || m_impl->scheduledCommands.contains(command) ||
+      (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled())) {
     return;
   }
 

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java

@@ -70,4 +70,18 @@ class SchedulerTest extends CommandTestBase {
       assertEquals(counter.get(), 2);
     }
   }
+
+  @Test
+  void scheduleScheduledNoOp() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+
+      Command command = Commands.startEnd(counter::incrementAndGet, () -> {});
+
+      scheduler.schedule(command);
+      scheduler.schedule(command);
+
+      assertEquals(counter.get(), 1);
+    }
+  }
 }

wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp

@@ -5,6 +5,7 @@
 #include "CommandTestBase.h"
 #include "frc2/command/InstantCommand.h"
 #include "frc2/command/RunCommand.h"
+#include "frc2/command/StartEndCommand.h"
 
 using namespace frc2;
 class SchedulerTest : public CommandTestBase {};
@@ -69,3 +70,16 @@ TEST_F(SchedulerTest, SchedulerCancelAll) {
 
   EXPECT_EQ(counter, 2);
 }
+
+TEST_F(SchedulerTest, ScheduleScheduledNoOp) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  StartEndCommand command([&counter] { counter++; }, [] {});
+
+  scheduler.Schedule(&command);
+  scheduler.Schedule(&command);
+
+  EXPECT_EQ(counter, 1);
+}