[commands] Add test for subsystem registration and periodic (#5408)

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

@@ -43,13 +43,41 @@ class SchedulerTest extends CommandTestBase {
     }
   }
 
+  @Test
+  void registerSubsystemTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger(0);
+      Subsystem system =
+          new SubsystemBase() {
+            @Override
+            public void periodic() {
+              counter.incrementAndGet();
+            }
+          };
+
+      assertDoesNotThrow(() -> scheduler.registerSubsystem(system));
+
+      scheduler.run();
+      assertEquals(1, counter.get());
+    }
+  }
+
   @Test
   void unregisterSubsystemTest() {
     try (CommandScheduler scheduler = new CommandScheduler()) {
-      Subsystem system = new SubsystemBase() {};
-
+      AtomicInteger counter = new AtomicInteger(0);
+      Subsystem system =
+          new SubsystemBase() {
+            @Override
+            public void periodic() {
+              counter.incrementAndGet();
+            }
+          };
       scheduler.registerSubsystem(system);
       assertDoesNotThrow(() -> scheduler.unregisterSubsystem(system));
+
+      scheduler.run();
+      assertEquals(0, counter.get());
     }
   }