SCRIPT Move subprojects

commandsv2/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java

@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import org.junit.jupiter.api.extension.BeforeAllCallback;
+import org.junit.jupiter.api.extension.ExtensionContext;
+import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
+
+public final class MockHardwareExtension implements BeforeAllCallback {
+  private static ExtensionContext getRoot(ExtensionContext context) {
+    return context.getParent().map(MockHardwareExtension::getRoot).orElse(context);
+  }
+
+  @Override
+  public void beforeAll(ExtensionContext context) {
+    getRoot(context)
+        .getStore(Namespace.GLOBAL)
+        .getOrComputeIfAbsent(
+            "HAL Initialized",
+            key -> {
+              initializeHardware();
+              return true;
+            },
+            Boolean.class);
+  }
+
+  private void initializeHardware() {
+    HAL.initialize(500, 0);
+    DriverStationSim.setDsAttached(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setTest(true);
+    DriverStationSim.notifyNewData();
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java

@@ -0,0 +1,539 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+class CommandDecoratorTest extends CommandTestBase {
+  @Test
+  @ResourceLock("timing")
+  void withTimeoutTest() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command timeout = Commands.idle().withTimeout(0.1);
+
+      scheduler.schedule(timeout);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(timeout));
+
+      SimHooks.stepTiming(0.15);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(timeout));
+    } finally {
+      SimHooks.resumeTiming();
+    }
+  }
+
+  @Test
+  void untilTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean finish = new AtomicBoolean();
+
+      Command command = Commands.idle().until(finish::get);
+
+      scheduler.schedule(command);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(command));
+
+      finish.set(true);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(command));
+    }
+  }
+
+  @Test
+  void untilOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean firstHasRun = new AtomicBoolean(false);
+      AtomicBoolean firstWasPolled = new AtomicBoolean(false);
+
+      Command first =
+          new FunctionalCommand(
+              () -> {},
+              () -> firstHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                firstWasPolled.set(true);
+                return true;
+              });
+      Command command =
+          first.until(
+              () -> {
+                assertAll(
+                    () -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+                return true;
+              });
+
+      scheduler.schedule(command);
+      scheduler.run();
+
+      assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+    }
+  }
+
+  @Test
+  void onlyWhileTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean run = new AtomicBoolean(true);
+
+      Command command = Commands.idle().onlyWhile(run::get);
+
+      scheduler.schedule(command);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(command));
+
+      run.set(false);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(command));
+    }
+  }
+
+  @Test
+  void onlyWhileOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean firstHasRun = new AtomicBoolean(false);
+      AtomicBoolean firstWasPolled = new AtomicBoolean(false);
+
+      Command first =
+          new FunctionalCommand(
+              () -> {},
+              () -> firstHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                firstWasPolled.set(true);
+                return true;
+              });
+      Command command =
+          first.onlyWhile(
+              () -> {
+                assertAll(
+                    () -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+                return false;
+              });
+
+      scheduler.schedule(command);
+      scheduler.run();
+
+      assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+    }
+  }
+
+  @Test
+  void ignoringDisableTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      var command = Commands.idle().ignoringDisable(true);
+
+      setDSEnabled(false);
+
+      scheduler.schedule(command);
+
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(command));
+    }
+  }
+
+  @Test
+  void beforeStartingTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean finished = new AtomicBoolean();
+      finished.set(false);
+
+      Command command = Commands.none().beforeStarting(() -> finished.set(true));
+
+      scheduler.schedule(command);
+
+      assertTrue(finished.get());
+
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(command));
+
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(command));
+    }
+  }
+
+  @Test
+  void andThenLambdaTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean finished = new AtomicBoolean(false);
+
+      Command command = Commands.none().andThen(() -> finished.set(true));
+
+      scheduler.schedule(command);
+
+      assertFalse(finished.get());
+
+      scheduler.run();
+
+      assertTrue(finished.get());
+
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(command));
+    }
+  }
+
+  @Test
+  void andThenTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean condition = new AtomicBoolean(false);
+
+      Command command1 = Commands.none();
+      Command command2 = Commands.runOnce(() -> condition.set(true));
+      Command group = command1.andThen(command2);
+
+      scheduler.schedule(group);
+
+      assertFalse(condition.get());
+
+      scheduler.run();
+
+      assertTrue(condition.get());
+
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void deadlineForTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean finish = new AtomicBoolean(false);
+
+      Command dictator = Commands.waitUntil(finish::get);
+      Command endsBefore = Commands.none();
+      Command endsAfter = Commands.idle();
+
+      Command group = dictator.deadlineFor(endsBefore, endsAfter);
+
+      scheduler.schedule(group);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(group));
+
+      finish.set(true);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void deadlineForOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean dictatorHasRun = new AtomicBoolean(false);
+      AtomicBoolean dictatorWasPolled = new AtomicBoolean(false);
+
+      Command dictator =
+          new FunctionalCommand(
+              () -> {},
+              () -> dictatorHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                dictatorWasPolled.set(true);
+                return true;
+              });
+      Command other =
+          Commands.run(
+              () ->
+                  assertAll(
+                      () -> assertTrue(dictatorHasRun.get()),
+                      () -> assertTrue(dictatorWasPolled.get())));
+
+      Command group = dictator.deadlineFor(other);
+
+      scheduler.schedule(group);
+      scheduler.run();
+
+      assertAll(() -> assertTrue(dictatorHasRun.get()), () -> assertTrue(dictatorWasPolled.get()));
+    }
+  }
+
+  @Test
+  void withDeadlineTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean finish = new AtomicBoolean(false);
+
+      Command endsBeforeGroup = Commands.none().withDeadline(Commands.waitUntil(finish::get));
+      scheduler.schedule(endsBeforeGroup);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(endsBeforeGroup));
+      finish.set(true);
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(endsBeforeGroup));
+      finish.set(false);
+
+      Command endsAfterGroup = Commands.idle().withDeadline(Commands.waitUntil(finish::get));
+      scheduler.schedule(endsAfterGroup);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(endsAfterGroup));
+      finish.set(true);
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(endsAfterGroup));
+    }
+  }
+
+  @Test
+  void withDeadlineOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean dictatorHasRun = new AtomicBoolean(false);
+      AtomicBoolean dictatorWasPolled = new AtomicBoolean(false);
+      Command dictator =
+          new FunctionalCommand(
+              () -> {},
+              () -> dictatorHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                dictatorWasPolled.set(true);
+                return true;
+              });
+      Command other =
+          Commands.run(
+              () ->
+                  assertAll(
+                      () -> assertTrue(dictatorHasRun.get()),
+                      () -> assertTrue(dictatorWasPolled.get())));
+      Command group = other.withDeadline(dictator);
+      scheduler.schedule(group);
+      scheduler.run();
+      assertAll(() -> assertTrue(dictatorHasRun.get()), () -> assertTrue(dictatorWasPolled.get()));
+    }
+  }
+
+  @Test
+  void alongWithTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean finish = new AtomicBoolean(false);
+
+      Command command1 = Commands.waitUntil(finish::get);
+      Command command2 = Commands.none();
+
+      Command group = command1.alongWith(command2);
+
+      scheduler.schedule(group);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(group));
+
+      finish.set(true);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void alongWithOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean firstHasRun = new AtomicBoolean(false);
+      AtomicBoolean firstWasPolled = new AtomicBoolean(false);
+
+      Command command1 =
+          new FunctionalCommand(
+              () -> {},
+              () -> firstHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                firstWasPolled.set(true);
+                return true;
+              });
+      Command command2 =
+          Commands.run(
+              () ->
+                  assertAll(
+                      () -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get())));
+
+      Command group = command1.alongWith(command2);
+
+      scheduler.schedule(group);
+      scheduler.run();
+
+      assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+    }
+  }
+
+  @Test
+  void raceWithTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command command1 = Commands.idle();
+      Command command2 = Commands.none();
+
+      Command group = command1.raceWith(command2);
+
+      scheduler.schedule(group);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void raceWithOrderTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean firstHasRun = new AtomicBoolean(false);
+      AtomicBoolean firstWasPolled = new AtomicBoolean(false);
+
+      Command command1 =
+          new FunctionalCommand(
+              () -> {},
+              () -> firstHasRun.set(true),
+              interrupted -> {},
+              () -> {
+                firstWasPolled.set(true);
+                return true;
+              });
+      Command command2 =
+          Commands.run(
+              () -> {
+                assertTrue(firstHasRun.get());
+                assertTrue(firstWasPolled.get());
+              });
+
+      Command group = command1.raceWith(command2);
+
+      scheduler.schedule(group);
+      scheduler.run();
+
+      assertAll(() -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()));
+    }
+  }
+
+  @Test
+  void unlessTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean hasRun = new AtomicBoolean(false);
+      AtomicBoolean unlessCondition = new AtomicBoolean(true);
+
+      Command command = Commands.runOnce(() -> hasRun.set(true)).unless(unlessCondition::get);
+
+      scheduler.schedule(command);
+      scheduler.run();
+      assertFalse(hasRun.get());
+
+      unlessCondition.set(false);
+      scheduler.schedule(command);
+      scheduler.run();
+      assertTrue(hasRun.get());
+    }
+  }
+
+  @Test
+  void onlyIfTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean hasRun = new AtomicBoolean(false);
+      AtomicBoolean onlyIfCondition = new AtomicBoolean(false);
+
+      Command command = Commands.runOnce(() -> hasRun.set(true)).onlyIf(onlyIfCondition::get);
+
+      scheduler.schedule(command);
+      scheduler.run();
+      assertFalse(hasRun.get());
+
+      onlyIfCondition.set(true);
+      scheduler.schedule(command);
+      scheduler.run();
+      assertTrue(hasRun.get());
+    }
+  }
+
+  @Test
+  void finallyDoTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger first = new AtomicInteger(0);
+      AtomicInteger second = new AtomicInteger(0);
+
+      Command command =
+          new FunctionalCommand(
+                  () -> {},
+                  () -> {},
+                  interrupted -> {
+                    if (!interrupted) {
+                      first.incrementAndGet();
+                    }
+                  },
+                  () -> true)
+              .finallyDo(
+                  interrupted -> {
+                    if (!interrupted) {
+                      // to differentiate between "didn't run" and "ran before command's `end()`
+                      second.addAndGet(1 + first.get());
+                    }
+                  });
+
+      scheduler.schedule(command);
+      assertEquals(0, first.get());
+      assertEquals(0, second.get());
+      scheduler.run();
+      assertEquals(1, first.get());
+      // if `second == 0`, neither of the lambdas ran.
+      // if `second == 1`, the second lambda ran before the first one
+      assertEquals(2, second.get());
+    }
+  }
+
+  // handleInterruptTest() implicitly tests the interrupt=true branch of finallyDo()
+  @Test
+  void handleInterruptTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger first = new AtomicInteger(0);
+      AtomicInteger second = new AtomicInteger(0);
+
+      Command command =
+          new FunctionalCommand(
+                  () -> {},
+                  () -> {},
+                  interrupted -> {
+                    if (interrupted) {
+                      first.incrementAndGet();
+                    }
+                  },
+                  () -> false)
+              .handleInterrupt(
+                  () -> {
+                    // to differentiate between "didn't run" and "ran before command's `end()`
+                    second.addAndGet(1 + first.get());
+                  });
+
+      scheduler.schedule(command);
+      scheduler.run();
+      assertEquals(0, first.get());
+      assertEquals(0, second.get());
+
+      scheduler.cancel(command);
+      assertEquals(1, first.get());
+      // if `second == 0`, neither of the lambdas ran.
+      // if `second == 1`, the second lambda ran before the first one
+      assertEquals(2, second.get());
+    }
+  }
+
+  @Test
+  void withNameTest() {
+    Command command = Commands.none();
+    String name = "Named";
+    Command named = command.withName(name);
+    assertEquals(name, named.getName());
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java

@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+import org.junit.jupiter.api.Test;
+
+class CommandRequirementsTest extends CommandTestBase {
+  @Test
+  void requirementInterruptTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Subsystem requirement = new SubsystemBase() {};
+
+      MockCommandHolder interruptedHolder = new MockCommandHolder(true, requirement);
+      Command interrupted = interruptedHolder.getMock();
+      MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
+      Command interrupter = interrupterHolder.getMock();
+
+      scheduler.schedule(interrupted);
+      scheduler.run();
+      scheduler.schedule(interrupter);
+      scheduler.run();
+
+      verify(interrupted).initialize();
+      verify(interrupted).execute();
+      verify(interrupted).end(true);
+
+      verify(interrupter).initialize();
+      verify(interrupter).execute();
+
+      assertFalse(scheduler.isScheduled(interrupted));
+      assertTrue(scheduler.isScheduled(interrupter));
+    }
+  }
+
+  @Test
+  void requirementUninterruptibleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Subsystem requirement = new SubsystemBase() {};
+
+      Command notInterrupted =
+          Commands.idle(requirement)
+              .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming);
+      MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement);
+      Command interrupter = interrupterHolder.getMock();
+
+      scheduler.schedule(notInterrupted);
+      scheduler.schedule(interrupter);
+
+      assertTrue(scheduler.isScheduled(notInterrupted));
+      assertFalse(scheduler.isScheduled(interrupter));
+    }
+  }
+
+  @Test
+  void defaultCommandRequirementErrorTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Subsystem system = new SubsystemBase() {};
+
+      Command missingRequirement = Commands.idle();
+
+      assertThrows(
+          IllegalArgumentException.class,
+          () -> scheduler.setDefaultCommand(system, missingRequirement));
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java

@@ -0,0 +1,144 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import org.junit.jupiter.api.Test;
+
+class CommandScheduleTest extends CommandTestBase {
+  @Test
+  void instantScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(true);
+      holder.setFinished(true);
+      Command mockCommand = holder.getMock();
+
+      scheduler.schedule(mockCommand);
+      assertTrue(scheduler.isScheduled(mockCommand));
+      verify(mockCommand).initialize();
+
+      scheduler.run();
+
+      verify(mockCommand).execute();
+      verify(mockCommand).end(false);
+
+      assertFalse(scheduler.isScheduled(mockCommand));
+    }
+  }
+
+  @Test
+  void singleIterationScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(true);
+      Command mockCommand = holder.getMock();
+
+      scheduler.schedule(mockCommand);
+
+      assertTrue(scheduler.isScheduled(mockCommand));
+
+      scheduler.run();
+      holder.setFinished(true);
+      scheduler.run();
+
+      verify(mockCommand).initialize();
+      verify(mockCommand, times(2)).execute();
+      verify(mockCommand).end(false);
+
+      assertFalse(scheduler.isScheduled(mockCommand));
+    }
+  }
+
+  @Test
+  void multiScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
+
+      scheduler.schedule(command1, command2, command3);
+      assertTrue(scheduler.isScheduled(command1, command2, command3));
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(command1, command2, command3));
+
+      command1Holder.setFinished(true);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(command2, command3));
+      assertFalse(scheduler.isScheduled(command1));
+
+      command2Holder.setFinished(true);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(command3));
+      assertFalse(scheduler.isScheduled(command1, command2));
+
+      command3Holder.setFinished(true);
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(command1, command2, command3));
+    }
+  }
+
+  @Test
+  void schedulerCancelTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(true);
+      Command mockCommand = holder.getMock();
+
+      scheduler.schedule(mockCommand);
+
+      scheduler.run();
+      scheduler.cancel(mockCommand);
+      scheduler.run();
+
+      verify(mockCommand).execute();
+      verify(mockCommand).end(true);
+      verify(mockCommand, never()).end(false);
+
+      assertFalse(scheduler.isScheduled(mockCommand));
+    }
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(true);
+      Command mockCommand = holder.getMock();
+
+      assertDoesNotThrow(() -> scheduler.cancel(mockCommand));
+    }
+  }
+
+  @Test
+  void smartDashboardCancelTest() {
+    try (CommandScheduler scheduler = new CommandScheduler();
+        var inst = NetworkTableInstance.create()) {
+      SmartDashboard.setNetworkTableInstance(inst);
+      SmartDashboard.putData("Scheduler", scheduler);
+      SmartDashboard.updateValues();
+
+      MockCommandHolder holder = new MockCommandHolder(true);
+      Command mockCommand = holder.getMock();
+      scheduler.schedule(mockCommand);
+      scheduler.run();
+      SmartDashboard.updateValues();
+      assertTrue(scheduler.isScheduled(mockCommand));
+
+      var table = inst.getTable("SmartDashboard");
+      table.getEntry("Scheduler/Cancel").setIntegerArray(new long[] {mockCommand.hashCode()});
+      SmartDashboard.updateValues();
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(mockCommand));
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/CommandSchedulerTest.java

@@ -0,0 +1,189 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertSame;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.Test;
+
+class CommandSchedulerTest extends CommandTestBase {
+  @Test
+  void schedulerLambdaTestNoInterrupt() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+
+      scheduler.onCommandInitialize(command -> counter.incrementAndGet());
+      scheduler.onCommandExecute(command -> counter.incrementAndGet());
+      scheduler.onCommandFinish(command -> counter.incrementAndGet());
+
+      scheduler.schedule(Commands.none());
+      scheduler.run();
+
+      assertEquals(counter.get(), 3);
+    }
+  }
+
+  @Test
+  void schedulerInterruptLambdaTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+
+      scheduler.onCommandInterrupt(command -> counter.incrementAndGet());
+
+      Command command = Commands.idle();
+
+      scheduler.schedule(command);
+      scheduler.cancel(command);
+
+      assertEquals(counter.get(), 1);
+    }
+  }
+
+  @Test
+  void schedulerInterruptNoCauseLambdaTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+
+      scheduler.onCommandInterrupt(
+          (interrupted, cause) -> {
+            assertFalse(cause.isPresent());
+            counter.incrementAndGet();
+          });
+
+      Command command = Commands.run(() -> {});
+
+      scheduler.schedule(command);
+      scheduler.cancel(command);
+
+      assertEquals(1, counter.get());
+    }
+  }
+
+  @Test
+  void schedulerInterruptCauseLambdaTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+
+      Subsystem subsystem = new Subsystem() {};
+      Command command = subsystem.run(() -> {});
+      Command interruptor = subsystem.runOnce(() -> {});
+
+      scheduler.onCommandInterrupt(
+          (interrupted, cause) -> {
+            assertTrue(cause.isPresent());
+            assertSame(interruptor, cause.get());
+            counter.incrementAndGet();
+          });
+
+      scheduler.schedule(command);
+      scheduler.schedule(interruptor);
+
+      assertEquals(1, counter.get());
+    }
+  }
+
+  @Test
+  void schedulerInterruptCauseLambdaInRunLoopTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+
+      Subsystem subsystem = new Subsystem() {};
+      Command command = subsystem.run(() -> {});
+      Command interruptor = subsystem.runOnce(() -> {});
+      // This command will schedule interruptor in execute() inside the run loop
+      Command interruptorScheduler = Commands.runOnce(() -> scheduler.schedule(interruptor));
+
+      scheduler.onCommandInterrupt(
+          (interrupted, cause) -> {
+            assertTrue(cause.isPresent());
+            assertSame(interruptor, cause.get());
+            counter.incrementAndGet();
+          });
+
+      scheduler.schedule(command);
+      scheduler.schedule(interruptorScheduler);
+
+      scheduler.run();
+
+      assertEquals(1, counter.get());
+    }
+  }
+
+  @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()) {
+      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());
+    }
+  }
+
+  @Test
+  void schedulerCancelAllTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+
+      scheduler.onCommandInterrupt(command -> counter.incrementAndGet());
+      scheduler.onCommandInterrupt((command, interruptor) -> assertFalse(interruptor.isPresent()));
+
+      Command command = Commands.idle();
+      Command command2 = Commands.idle();
+
+      scheduler.schedule(command);
+      scheduler.schedule(command2);
+      scheduler.cancelAll();
+
+      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);
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/CommandSendableButtonTest.java

@@ -0,0 +1,114 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.networktables.BooleanPublisher;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class CommandSendableButtonTest extends CommandTestBase {
+  private NetworkTableInstance m_inst;
+  private AtomicInteger m_schedule;
+  private AtomicInteger m_cancel;
+  private BooleanPublisher m_publish;
+  private Command m_command;
+
+  @BeforeEach
+  void setUp() {
+    m_inst = NetworkTableInstance.create();
+    SmartDashboard.setNetworkTableInstance(m_inst);
+    m_schedule = new AtomicInteger();
+    m_cancel = new AtomicInteger();
+    m_command = Commands.startEnd(m_schedule::incrementAndGet, m_cancel::incrementAndGet);
+    m_publish = m_inst.getBooleanTopic("/SmartDashboard/command/running").publish();
+    SmartDashboard.putData("command", m_command);
+    SmartDashboard.updateValues();
+  }
+
+  @Test
+  void trueAndNotScheduledSchedules() {
+    // Not scheduled and true -> scheduled
+    CommandScheduler.getInstance().run();
+    SmartDashboard.updateValues();
+    assertFalse(m_command.isScheduled());
+    assertEquals(0, m_schedule.get());
+    assertEquals(0, m_cancel.get());
+
+    m_publish.set(true);
+    SmartDashboard.updateValues();
+    CommandScheduler.getInstance().run();
+    assertTrue(m_command.isScheduled());
+    assertEquals(1, m_schedule.get());
+    assertEquals(0, m_cancel.get());
+  }
+
+  @Test
+  void trueAndScheduledNoOp() {
+    // Scheduled and true -> no-op
+    CommandScheduler.getInstance().schedule(m_command);
+    CommandScheduler.getInstance().run();
+    SmartDashboard.updateValues();
+    assertTrue(m_command.isScheduled());
+    assertEquals(1, m_schedule.get());
+    assertEquals(0, m_cancel.get());
+
+    m_publish.set(true);
+    SmartDashboard.updateValues();
+    CommandScheduler.getInstance().run();
+    assertTrue(m_command.isScheduled());
+    assertEquals(1, m_schedule.get());
+    assertEquals(0, m_cancel.get());
+  }
+
+  @Test
+  void falseAndNotScheduledNoOp() {
+    // Not scheduled and false -> no-op
+    CommandScheduler.getInstance().run();
+    SmartDashboard.updateValues();
+    assertFalse(m_command.isScheduled());
+    assertEquals(0, m_schedule.get());
+    assertEquals(0, m_cancel.get());
+
+    m_publish.set(false);
+    SmartDashboard.updateValues();
+    CommandScheduler.getInstance().run();
+    assertFalse(m_command.isScheduled());
+    assertEquals(0, m_schedule.get());
+    assertEquals(0, m_cancel.get());
+  }
+
+  @Test
+  void falseAndScheduledCancel() {
+    // Scheduled and false -> cancel
+    CommandScheduler.getInstance().schedule(m_command);
+    CommandScheduler.getInstance().run();
+    SmartDashboard.updateValues();
+    assertTrue(m_command.isScheduled());
+    assertEquals(1, m_schedule.get());
+    assertEquals(0, m_cancel.get());
+
+    m_publish.set(false);
+    SmartDashboard.updateValues();
+    CommandScheduler.getInstance().run();
+    assertFalse(m_command.isScheduled());
+    assertEquals(1, m_schedule.get());
+    assertEquals(1, m_cancel.get());
+  }
+
+  @AfterEach
+  void tearDown() {
+    m_publish.close();
+    m_inst.close();
+    SmartDashboard.setNetworkTableInstance(NetworkTableInstance.getDefault());
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.when;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
+import java.util.Set;
+import org.junit.jupiter.api.BeforeEach;
+
+/** Basic setup for all {@link Command tests}. */
+public class CommandTestBase {
+  protected CommandTestBase() {}
+
+  @BeforeEach
+  void commandSetup() {
+    CommandScheduler.getInstance().cancelAll();
+    CommandScheduler.getInstance().enable();
+    CommandScheduler.getInstance().getActiveButtonLoop().clear();
+    CommandScheduler.getInstance().clearComposedCommands();
+    CommandScheduler.getInstance().unregisterAllSubsystems();
+
+    setDSEnabled(true);
+  }
+
+  public void setDSEnabled(boolean enabled) {
+    DriverStationSim.setDsAttached(true);
+
+    DriverStationSim.setEnabled(enabled);
+    DriverStationSim.notifyNewData();
+    while (DriverStation.isEnabled() != enabled) {
+      try {
+        Thread.sleep(1);
+      } catch (InterruptedException exception) {
+        exception.printStackTrace();
+      }
+    }
+  }
+
+  public static class MockCommandHolder {
+    private final Command m_mockCommand = mock(Command.class);
+
+    public MockCommandHolder(boolean runWhenDisabled, Subsystem... requirements) {
+      when(m_mockCommand.getRequirements()).thenReturn(Set.of(requirements));
+      when(m_mockCommand.isFinished()).thenReturn(false);
+      when(m_mockCommand.runsWhenDisabled()).thenReturn(runWhenDisabled);
+      when(m_mockCommand.getInterruptionBehavior()).thenReturn(InterruptionBehavior.kCancelSelf);
+    }
+
+    public Command getMock() {
+      return m_mockCommand;
+    }
+
+    public void setFinished(boolean finished) {
+      when(m_mockCommand.isFinished()).thenReturn(finished);
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java

@@ -0,0 +1,151 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
+import java.util.function.BooleanSupplier;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class ConditionalCommandTest extends CommandTestBase {
+  @Test
+  void conditionalCommandTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      command1Holder.setFinished(true);
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+
+      ConditionalCommand conditionalCommand =
+          new ConditionalCommand(command1, command2, () -> true);
+
+      scheduler.schedule(conditionalCommand);
+      scheduler.run();
+
+      verify(command1).initialize();
+      verify(command1).execute();
+      verify(command1).end(false);
+
+      verify(command2, never()).initialize();
+      verify(command2, never()).execute();
+      verify(command2, never()).end(false);
+    }
+  }
+
+  @Test
+  void conditionalCommandRequirementTest() {
+    Subsystem system1 = new SubsystemBase() {};
+    Subsystem system2 = new SubsystemBase() {};
+    Subsystem system3 = new SubsystemBase() {};
+
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
+
+      ConditionalCommand conditionalCommand =
+          new ConditionalCommand(command1, command2, () -> true);
+
+      scheduler.schedule(conditionalCommand);
+      scheduler.schedule(new InstantCommand(() -> {}, system3));
+
+      assertFalse(scheduler.isScheduled(conditionalCommand));
+
+      verify(command1).end(true);
+      verify(command2, never()).end(true);
+    }
+  }
+
+  static Stream<Arguments> interruptible() {
+    return Stream.of(
+        arguments(
+            "AllCancelSelf",
+            InterruptionBehavior.kCancelSelf,
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf),
+            (BooleanSupplier) () -> true),
+        arguments(
+            "AllCancelIncoming",
+            InterruptionBehavior.kCancelIncoming,
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming),
+            (BooleanSupplier) () -> true),
+        arguments(
+            "OneCancelSelfOneIncoming",
+            InterruptionBehavior.kCancelSelf,
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming),
+            (BooleanSupplier) () -> true),
+        arguments(
+            "OneCancelIncomingOneSelf",
+            InterruptionBehavior.kCancelSelf,
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf),
+            (BooleanSupplier) () -> true));
+  }
+
+  @MethodSource
+  @ParameterizedTest(name = "interruptible[{index}]: {0}")
+  void interruptible(
+      @SuppressWarnings("unused") String name,
+      InterruptionBehavior expected,
+      Command command1,
+      Command command2,
+      BooleanSupplier selector) {
+    var command = Commands.either(command1, command2, selector);
+    assertEquals(expected, command.getInterruptionBehavior());
+  }
+
+  static Stream<Arguments> runsWhenDisabled() {
+    return Stream.of(
+        arguments(
+            "AllFalse",
+            false,
+            Commands.idle().ignoringDisable(false),
+            Commands.idle().ignoringDisable(false),
+            (BooleanSupplier) () -> true),
+        arguments(
+            "AllTrue",
+            true,
+            Commands.idle().ignoringDisable(true),
+            Commands.idle().ignoringDisable(true),
+            (BooleanSupplier) () -> true),
+        arguments(
+            "OneTrueOneFalse",
+            false,
+            Commands.idle().ignoringDisable(true),
+            Commands.idle().ignoringDisable(false),
+            (BooleanSupplier) () -> true),
+        arguments(
+            "OneFalseOneTrue",
+            false,
+            Commands.idle().ignoringDisable(false),
+            Commands.idle().ignoringDisable(true),
+            (BooleanSupplier) () -> true));
+  }
+
+  @MethodSource
+  @ParameterizedTest(name = "runsWhenDisabled[{index}]: {0}")
+  void runsWhenDisabled(
+      @SuppressWarnings("unused") String name,
+      boolean expected,
+      Command command1,
+      Command command2,
+      BooleanSupplier selector) {
+    var command = Commands.either(command1, command2, selector);
+    assertEquals(expected, command.runsWhenDisabled());
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java

@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+import org.junit.jupiter.api.Test;
+
+class DefaultCommandTest extends CommandTestBase {
+  @Test
+  void defaultCommandScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Subsystem hasDefaultCommand = new SubsystemBase() {};
+
+      MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
+      Command defaultCommand = defaultHolder.getMock();
+
+      scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(defaultCommand));
+    }
+  }
+
+  @Test
+  void defaultCommandInterruptResumeTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Subsystem hasDefaultCommand = new SubsystemBase() {};
+
+      MockCommandHolder defaultHolder = new MockCommandHolder(true, hasDefaultCommand);
+      Command defaultCommand = defaultHolder.getMock();
+      MockCommandHolder interrupterHolder = new MockCommandHolder(true, hasDefaultCommand);
+      Command interrupter = interrupterHolder.getMock();
+
+      scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+      scheduler.run();
+      scheduler.schedule(interrupter);
+
+      assertFalse(scheduler.isScheduled(defaultCommand));
+      assertTrue(scheduler.isScheduled(interrupter));
+
+      scheduler.cancel(interrupter);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(defaultCommand));
+      assertFalse(scheduler.isScheduled(interrupter));
+    }
+  }
+
+  @Test
+  void defaultCommandDisableResumeTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Subsystem hasDefaultCommand = new SubsystemBase() {};
+
+      MockCommandHolder defaultHolder = new MockCommandHolder(false, hasDefaultCommand);
+      Command defaultCommand = defaultHolder.getMock();
+
+      scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(defaultCommand));
+
+      setDSEnabled(false);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(defaultCommand));
+
+      setDSEnabled(true);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(defaultCommand));
+
+      verify(defaultCommand).end(true);
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/DeferredCommandTest.java

@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.only;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+import java.util.Set;
+import java.util.function.Supplier;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.ValueSource;
+
+class DeferredCommandTest extends CommandTestBase {
+  @ParameterizedTest
+  @ValueSource(booleans = {true, false})
+  void deferredFunctionsTest(boolean interrupted) {
+    MockCommandHolder innerCommand = new MockCommandHolder(false);
+    DeferredCommand command = new DeferredCommand(innerCommand::getMock, Set.of());
+
+    command.initialize();
+    verify(innerCommand.getMock()).initialize();
+
+    command.execute();
+    verify(innerCommand.getMock()).execute();
+
+    assertFalse(command.isFinished());
+    verify(innerCommand.getMock()).isFinished();
+
+    innerCommand.setFinished(true);
+    assertTrue(command.isFinished());
+    verify(innerCommand.getMock(), times(2)).isFinished();
+
+    command.end(interrupted);
+    verify(innerCommand.getMock()).end(interrupted);
+  }
+
+  @SuppressWarnings("unchecked")
+  @Test
+  void deferredSupplierOnlyCalledDuringInit() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Supplier<Command> supplier = (Supplier<Command>) mock(Supplier.class);
+      when(supplier.get()).thenReturn(Commands.none(), Commands.none());
+
+      DeferredCommand command = new DeferredCommand(supplier, Set.of());
+      verify(supplier, never()).get();
+
+      scheduler.schedule(command);
+      verify(supplier, only()).get();
+      scheduler.run();
+
+      scheduler.schedule(command);
+      verify(supplier, times(2)).get();
+    }
+  }
+
+  @Test
+  void deferredRequirementsTest() {
+    Subsystem subsystem = new Subsystem() {};
+    DeferredCommand command = new DeferredCommand(Commands::none, Set.of(subsystem));
+
+    assertTrue(command.getRequirements().contains(subsystem));
+  }
+
+  @Test
+  void deferredNullCommandTest() {
+    DeferredCommand command = new DeferredCommand(() -> null, Set.of());
+    assertDoesNotThrow(
+        () -> {
+          command.initialize();
+          command.execute();
+          command.isFinished();
+          command.end(false);
+        });
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.concurrent.atomic.AtomicBoolean;
+import org.junit.jupiter.api.Test;
+
+class FunctionalCommandTest extends CommandTestBase {
+  @Test
+  void functionalCommandScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean cond1 = new AtomicBoolean();
+      AtomicBoolean cond2 = new AtomicBoolean();
+      AtomicBoolean cond3 = new AtomicBoolean();
+      AtomicBoolean cond4 = new AtomicBoolean();
+
+      FunctionalCommand command =
+          new FunctionalCommand(
+              () -> cond1.set(true),
+              () -> cond2.set(true),
+              interrupted -> cond3.set(true),
+              cond4::get);
+
+      scheduler.schedule(command);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(command));
+
+      cond4.set(true);
+
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(command));
+      assertTrue(cond1.get());
+      assertTrue(cond2.get());
+      assertTrue(cond3.get());
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.concurrent.atomic.AtomicBoolean;
+import org.junit.jupiter.api.Test;
+
+class InstantCommandTest extends CommandTestBase {
+  @Test
+  void instantCommandScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean cond = new AtomicBoolean();
+
+      InstantCommand command = new InstantCommand(() -> cond.set(true));
+
+      scheduler.schedule(command);
+      scheduler.run();
+
+      assertTrue(cond.get());
+      assertFalse(scheduler.isScheduled(command));
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java

@@ -0,0 +1,119 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
+import java.util.stream.Stream;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+abstract class MultiCompositionTestBase<T extends Command> extends SingleCompositionTestBase<T> {
+  abstract T compose(Command... members);
+
+  @Override
+  T composeSingle(Command member) {
+    return compose(member);
+  }
+
+  static Stream<Arguments> interruptible() {
+    return Stream.of(
+        arguments(
+            "AllCancelSelf",
+            InterruptionBehavior.kCancelSelf,
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf)),
+        arguments(
+            "AllCancelIncoming",
+            InterruptionBehavior.kCancelIncoming,
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming)),
+        arguments(
+            "TwoCancelSelfOneIncoming",
+            InterruptionBehavior.kCancelSelf,
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming)),
+        arguments(
+            "TwoCancelIncomingOneSelf",
+            InterruptionBehavior.kCancelSelf,
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming),
+            Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf)));
+  }
+
+  @MethodSource
+  @ParameterizedTest(name = "interruptible[{index}]: {0}")
+  void interruptible(
+      @SuppressWarnings("unused") String name,
+      InterruptionBehavior expected,
+      Command command1,
+      Command command2,
+      Command command3) {
+    var command = compose(command1, command2, command3);
+    assertEquals(expected, command.getInterruptionBehavior());
+  }
+
+  static Stream<Arguments> runsWhenDisabled() {
+    return Stream.of(
+        arguments(
+            "AllFalse",
+            false,
+            Commands.idle().ignoringDisable(false),
+            Commands.idle().ignoringDisable(false),
+            Commands.idle().ignoringDisable(false)),
+        arguments(
+            "AllTrue",
+            true,
+            Commands.idle().ignoringDisable(true),
+            Commands.idle().ignoringDisable(true),
+            Commands.idle().ignoringDisable(true)),
+        arguments(
+            "TwoTrueOneFalse",
+            false,
+            Commands.idle().ignoringDisable(true),
+            Commands.idle().ignoringDisable(true),
+            Commands.idle().ignoringDisable(false)),
+        arguments(
+            "TwoFalseOneTrue",
+            false,
+            Commands.idle().ignoringDisable(false),
+            Commands.idle().ignoringDisable(false),
+            Commands.idle().ignoringDisable(true)));
+  }
+
+  @MethodSource
+  @ParameterizedTest(name = "runsWhenDisabled[{index}]: {0}")
+  void runsWhenDisabled(
+      @SuppressWarnings("unused") String name,
+      boolean expected,
+      Command command1,
+      Command command2,
+      Command command3) {
+    var command = compose(command1, command2, command3);
+    assertEquals(expected, command.runsWhenDisabled());
+  }
+
+  static Stream<Arguments> composeDuplicates() {
+    Command a = Commands.none();
+    Command b = Commands.none();
+    return Stream.of(
+        arguments("AA", new Command[] {a, a}),
+        arguments("ABA", new Command[] {a, b, a}),
+        arguments("BAA", new Command[] {b, a, a}));
+  }
+
+  @MethodSource
+  @ParameterizedTest(name = "composeDuplicates[{index}]: {0}")
+  void composeDuplicates(@SuppressWarnings("unused") String name, Command[] commands) {
+    assertThrows(IllegalArgumentException.class, () -> compose(commands));
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+class NotifierCommandTest extends CommandTestBase {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void notifierCommandScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger(0);
+
+      NotifierCommand command = new NotifierCommand(counter::incrementAndGet, 0.01);
+
+      scheduler.schedule(command);
+      for (int i = 0; i < 5; ++i) {
+        SimHooks.stepTiming(0.005);
+      }
+      scheduler.cancel(command);
+
+      assertEquals(2, counter.get());
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java

@@ -0,0 +1,134 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+import org.junit.jupiter.api.Test;
+
+class ParallelCommandGroupTest extends MultiCompositionTestBase<ParallelCommandGroup> {
+  @Test
+  void parallelGroupScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+
+      Command group = new ParallelCommandGroup(command1, command2);
+
+      scheduler.schedule(group);
+
+      verify(command1).initialize();
+      verify(command2).initialize();
+
+      command1Holder.setFinished(true);
+      scheduler.run();
+      command2Holder.setFinished(true);
+      scheduler.run();
+
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command2, times(2)).execute();
+      verify(command2).end(false);
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void parallelGroupInterruptTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+
+      Command group = new ParallelCommandGroup(command1, command2);
+
+      scheduler.schedule(group);
+
+      command1Holder.setFinished(true);
+      scheduler.run();
+      scheduler.run();
+      scheduler.cancel(group);
+
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command1, never()).end(true);
+
+      verify(command2, times(2)).execute();
+      verify(command2, never()).end(false);
+      verify(command2).end(true);
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+
+      Command group = new ParallelCommandGroup(command1, command2);
+
+      assertDoesNotThrow(() -> scheduler.cancel(group));
+    }
+  }
+
+  @Test
+  void parallelGroupRequirementTest() {
+    Subsystem system1 = new SubsystemBase() {};
+    Subsystem system2 = new SubsystemBase() {};
+    Subsystem system3 = new SubsystemBase() {};
+    Subsystem system4 = new SubsystemBase() {};
+
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+      Command command3 = command3Holder.getMock();
+
+      Command group = new ParallelCommandGroup(command1, command2);
+
+      scheduler.schedule(group);
+      scheduler.schedule(command3);
+
+      assertFalse(scheduler.isScheduled(group));
+      assertTrue(scheduler.isScheduled(command3));
+    }
+  }
+
+  @Test
+  void parallelGroupRequirementErrorTest() {
+    Subsystem system1 = new SubsystemBase() {};
+    Subsystem system2 = new SubsystemBase() {};
+    Subsystem system3 = new SubsystemBase() {};
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+    Command command2 = command2Holder.getMock();
+
+    assertThrows(
+        IllegalArgumentException.class, () -> new ParallelCommandGroup(command1, command2));
+  }
+
+  @Override
+  public ParallelCommandGroup compose(Command... members) {
+    return new ParallelCommandGroup(members);
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java

@@ -0,0 +1,147 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+import static org.mockito.internal.verification.VerificationModeFactory.times;
+
+import java.util.Arrays;
+import org.junit.jupiter.api.Test;
+
+class ParallelDeadlineGroupTest extends MultiCompositionTestBase<ParallelDeadlineGroup> {
+  @Test
+  void parallelDeadlineScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      command2Holder.setFinished(true);
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
+
+      Command group = new ParallelDeadlineGroup(command1, command2, command3);
+
+      scheduler.schedule(group);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(group));
+
+      command1Holder.setFinished(true);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(group));
+
+      verify(command2).initialize();
+      verify(command2).execute();
+      verify(command2).end(false);
+      verify(command2, never()).end(true);
+
+      verify(command1).initialize();
+      verify(command1, times(2)).execute();
+      verify(command1).end(false);
+      verify(command1, never()).end(true);
+
+      verify(command3).initialize();
+      verify(command3, times(2)).execute();
+      verify(command3, never()).end(false);
+      verify(command3).end(true);
+    }
+  }
+
+  @Test
+  void parallelDeadlineInterruptTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      command2Holder.setFinished(true);
+
+      Command group = new ParallelDeadlineGroup(command1, command2);
+
+      scheduler.schedule(group);
+
+      scheduler.run();
+      scheduler.run();
+      scheduler.cancel(group);
+
+      verify(command1, times(2)).execute();
+      verify(command1, never()).end(false);
+      verify(command1).end(true);
+
+      verify(command2).execute();
+      verify(command2).end(false);
+      verify(command2, never()).end(true);
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void parallelDeadlineRequirementTest() {
+    Subsystem system1 = new SubsystemBase() {};
+    Subsystem system2 = new SubsystemBase() {};
+    Subsystem system3 = new SubsystemBase() {};
+    Subsystem system4 = new SubsystemBase() {};
+
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+      Command command3 = command3Holder.getMock();
+
+      Command group = new ParallelDeadlineGroup(command1, command2);
+
+      scheduler.schedule(group);
+      scheduler.schedule(command3);
+
+      assertFalse(scheduler.isScheduled(group));
+      assertTrue(scheduler.isScheduled(command3));
+    }
+  }
+
+  @Test
+  void parallelDeadlineRequirementErrorTest() {
+    Subsystem system1 = new SubsystemBase() {};
+    Subsystem system2 = new SubsystemBase() {};
+    Subsystem system3 = new SubsystemBase() {};
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+    Command command2 = command2Holder.getMock();
+
+    assertThrows(
+        IllegalArgumentException.class, () -> new ParallelDeadlineGroup(command1, command2));
+  }
+
+  @Test
+  void parallelDeadlineSetDeadlineToDeadlineTest() {
+    Command a = Commands.none();
+    ParallelDeadlineGroup group = new ParallelDeadlineGroup(a);
+    assertDoesNotThrow(() -> group.setDeadline(a));
+  }
+
+  @Test
+  void parallelDeadlineSetDeadlineDuplicateTest() {
+    Command a = Commands.none();
+    Command b = Commands.none();
+    ParallelDeadlineGroup group = new ParallelDeadlineGroup(a, b);
+    assertThrows(IllegalArgumentException.class, () -> group.setDeadline(b));
+  }
+
+  @Override
+  public ParallelDeadlineGroup compose(Command... members) {
+    return new ParallelDeadlineGroup(members[0], Arrays.copyOfRange(members, 1, members.length));
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java

@@ -0,0 +1,210 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.reset;
+import static org.mockito.Mockito.times;
+import static org.mockito.Mockito.verify;
+
+import org.junit.jupiter.api.Test;
+
+class ParallelRaceGroupTest extends MultiCompositionTestBase<ParallelRaceGroup> {
+  @Test
+  void parallelRaceScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+
+      Command group = new ParallelRaceGroup(command1, command2);
+
+      scheduler.schedule(group);
+
+      verify(command1).initialize();
+      verify(command2).initialize();
+
+      command1Holder.setFinished(true);
+      scheduler.run();
+      command2Holder.setFinished(true);
+      scheduler.run();
+
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command2).execute();
+      verify(command2).end(true);
+      verify(command2, never()).end(false);
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void parallelRaceInterruptTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+
+      Command group = new ParallelRaceGroup(command1, command2);
+
+      scheduler.schedule(group);
+
+      scheduler.run();
+      scheduler.run();
+      scheduler.cancel(group);
+
+      verify(command1, times(2)).execute();
+      verify(command1, never()).end(false);
+      verify(command1).end(true);
+
+      verify(command2, times(2)).execute();
+      verify(command2, never()).end(false);
+      verify(command2).end(true);
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+
+      Command group = new ParallelRaceGroup(command1, command2);
+
+      assertDoesNotThrow(() -> scheduler.cancel(group));
+    }
+  }
+
+  @Test
+  void parallelRaceRequirementTest() {
+    Subsystem system1 = new SubsystemBase() {};
+    Subsystem system2 = new SubsystemBase() {};
+    Subsystem system3 = new SubsystemBase() {};
+    Subsystem system4 = new SubsystemBase() {};
+
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+      Command command3 = command3Holder.getMock();
+
+      Command group = new ParallelRaceGroup(command1, command2);
+
+      scheduler.schedule(group);
+      scheduler.schedule(command3);
+
+      assertFalse(scheduler.isScheduled(group));
+      assertTrue(scheduler.isScheduled(command3));
+    }
+  }
+
+  @Test
+  void parallelRaceRequirementErrorTest() {
+    Subsystem system1 = new SubsystemBase() {};
+    Subsystem system2 = new SubsystemBase() {};
+    Subsystem system3 = new SubsystemBase() {};
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
+    Command command2 = command2Holder.getMock();
+
+    assertThrows(IllegalArgumentException.class, () -> new ParallelRaceGroup(command1, command2));
+  }
+
+  @Test
+  void parallelRaceOnlyCallsEndOnceTest() {
+    Subsystem system1 = new SubsystemBase() {};
+    Subsystem system2 = new SubsystemBase() {};
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true, system1);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true, system2);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+
+    Command group1 = Commands.sequence(command1, command2);
+    assertNotNull(group1);
+    assertNotNull(command3);
+    Command group2 = new ParallelRaceGroup(group1, command3);
+
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      scheduler.schedule(group2);
+      scheduler.run();
+      command1Holder.setFinished(true);
+      scheduler.run();
+      command2Holder.setFinished(true);
+      // at this point the sequential group should be done
+      assertDoesNotThrow(scheduler::run);
+      assertFalse(scheduler.isScheduled(group2));
+    }
+  }
+
+  @Test
+  void parallelRaceScheduleTwiceTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+
+      Command group = new ParallelRaceGroup(command1, command2);
+
+      scheduler.schedule(group);
+
+      verify(command1).initialize();
+      verify(command2).initialize();
+
+      command1Holder.setFinished(true);
+      scheduler.run();
+      command2Holder.setFinished(true);
+      scheduler.run();
+
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command2).execute();
+      verify(command2).end(true);
+      verify(command2, never()).end(false);
+
+      assertFalse(scheduler.isScheduled(group));
+
+      reset(command1);
+      reset(command2);
+
+      scheduler.schedule(group);
+
+      verify(command1).initialize();
+      verify(command2).initialize();
+
+      scheduler.run();
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(group));
+      command2Holder.setFinished(true);
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Override
+  public ParallelRaceGroup compose(Command... members) {
+    return new ParallelRaceGroup(members);
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+
+import java.io.ByteArrayOutputStream;
+import java.io.PrintStream;
+import org.junit.jupiter.api.Test;
+
+class PrintCommandTest extends CommandTestBase {
+  @Test
+  void printCommandScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      final PrintStream originalOut = System.out;
+      ByteArrayOutputStream testOut = new ByteArrayOutputStream();
+      System.setOut(new PrintStream(testOut));
+      try {
+        PrintCommand command = new PrintCommand("Test!");
+
+        scheduler.schedule(command);
+        scheduler.run();
+
+        assertFalse(scheduler.isScheduled(command));
+        assertEquals(testOut.toString(), "Test!" + System.lineSeparator());
+      } finally {
+        System.setOut(originalOut);
+      }
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/ProxyCommandTest.java

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.verify;
+
+import java.util.concurrent.atomic.AtomicBoolean;
+import org.junit.jupiter.api.Test;
+
+class ProxyCommandTest extends CommandTestBase {
+  @Test
+  void proxyCommandScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+
+      ProxyCommand scheduleCommand = new ProxyCommand(command1);
+
+      scheduler.schedule(scheduleCommand);
+
+      verify(command1).initialize();
+    }
+  }
+
+  @Test
+  void proxyCommandEndTest() {
+    try (CommandScheduler scheduler = CommandScheduler.getInstance()) {
+      AtomicBoolean cond = new AtomicBoolean();
+
+      Command command = Commands.waitUntil(cond::get);
+
+      ProxyCommand scheduleCommand = new ProxyCommand(command);
+
+      scheduler.schedule(scheduleCommand);
+
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(scheduleCommand));
+
+      cond.set(true);
+      scheduler.run();
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(scheduleCommand));
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/RepeatCommandTest.java

@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.Test;
+
+class RepeatCommandTest extends SingleCompositionTestBase<RepeatCommand> {
+  @Test
+  void callsMethodsCorrectly() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      var initCounter = new AtomicInteger(0);
+      var exeCounter = new AtomicInteger(0);
+      var isFinishedCounter = new AtomicInteger(0);
+      var endCounter = new AtomicInteger(0);
+      var isFinishedHook = new AtomicBoolean(false);
+
+      final var command =
+          new FunctionalCommand(
+                  initCounter::incrementAndGet,
+                  exeCounter::incrementAndGet,
+                  interrupted -> endCounter.incrementAndGet(),
+                  () -> {
+                    isFinishedCounter.incrementAndGet();
+                    return isFinishedHook.get();
+                  })
+              .repeatedly();
+
+      assertEquals(0, initCounter.get());
+      assertEquals(0, exeCounter.get());
+      assertEquals(0, isFinishedCounter.get());
+      assertEquals(0, endCounter.get());
+
+      scheduler.schedule(command);
+      assertEquals(1, initCounter.get());
+      assertEquals(0, exeCounter.get());
+      assertEquals(0, isFinishedCounter.get());
+      assertEquals(0, endCounter.get());
+
+      isFinishedHook.set(false);
+      scheduler.run();
+      assertEquals(1, initCounter.get());
+      assertEquals(1, exeCounter.get());
+      assertEquals(1, isFinishedCounter.get());
+      assertEquals(0, endCounter.get());
+
+      isFinishedHook.set(true);
+      scheduler.run();
+      assertEquals(1, initCounter.get());
+      assertEquals(2, exeCounter.get());
+      assertEquals(2, isFinishedCounter.get());
+      assertEquals(1, endCounter.get());
+
+      isFinishedHook.set(false);
+      scheduler.run();
+      assertEquals(2, initCounter.get());
+      assertEquals(3, exeCounter.get());
+      assertEquals(3, isFinishedCounter.get());
+      assertEquals(1, endCounter.get());
+
+      isFinishedHook.set(true);
+      scheduler.run();
+      assertEquals(2, initCounter.get());
+      assertEquals(4, exeCounter.get());
+      assertEquals(4, isFinishedCounter.get());
+      assertEquals(2, endCounter.get());
+
+      scheduler.cancel(command);
+      assertEquals(2, initCounter.get());
+      assertEquals(4, exeCounter.get());
+      assertEquals(4, isFinishedCounter.get());
+      assertEquals(2, endCounter.get());
+    }
+  }
+
+  @Override
+  public RepeatCommand composeSingle(Command member) {
+    return member.repeatedly();
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java

@@ -0,0 +1,179 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static edu.wpi.first.wpilibj2.command.Commands.parallel;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.Map;
+import org.junit.jupiter.api.Test;
+
+class RobotDisabledCommandTest extends CommandTestBase {
+  @Test
+  void robotDisabledCommandCancelTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(false);
+      Command mockCommand = holder.getMock();
+
+      scheduler.schedule(mockCommand);
+
+      assertTrue(scheduler.isScheduled(mockCommand));
+
+      setDSEnabled(false);
+
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(mockCommand));
+
+      setDSEnabled(true);
+    }
+  }
+
+  @Test
+  void runWhenDisabledTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder holder = new MockCommandHolder(true);
+      Command mockCommand = holder.getMock();
+
+      scheduler.schedule(mockCommand);
+
+      assertTrue(scheduler.isScheduled(mockCommand));
+
+      setDSEnabled(false);
+
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(mockCommand));
+    }
+  }
+
+  @Test
+  void sequentialGroupRunWhenDisabledTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
+      MockCommandHolder command4Holder = new MockCommandHolder(false);
+      Command command4 = command4Holder.getMock();
+
+      Command runWhenDisabled = new SequentialCommandGroup(command1, command2);
+      Command dontRunWhenDisabled = new SequentialCommandGroup(command3, command4);
+
+      scheduler.schedule(runWhenDisabled);
+      scheduler.schedule(dontRunWhenDisabled);
+
+      setDSEnabled(false);
+
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(runWhenDisabled));
+      assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+    }
+  }
+
+  @Test
+  void parallelGroupRunWhenDisabledTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
+      MockCommandHolder command4Holder = new MockCommandHolder(false);
+      Command command4 = command4Holder.getMock();
+
+      Command runWhenDisabled = new ParallelCommandGroup(command1, command2);
+      Command dontRunWhenDisabled = new ParallelCommandGroup(command3, command4);
+
+      scheduler.schedule(runWhenDisabled);
+      scheduler.schedule(dontRunWhenDisabled);
+
+      setDSEnabled(false);
+
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(runWhenDisabled));
+      assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+    }
+  }
+
+  @Test
+  void conditionalRunWhenDisabledTest() {
+    setDSEnabled(false);
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
+      Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
+
+      scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
+
+      assertTrue(scheduler.isScheduled(runWhenDisabled));
+      assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+    }
+  }
+
+  @Test
+  void selectRunWhenDisabledTest() {
+    setDSEnabled(false);
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    Command runWhenDisabled = new SelectCommand<>(Map.of(1, command1, 2, command2), () -> 1);
+    Command dontRunWhenDisabled = new SelectCommand<>(Map.of(1, command3, 2, command4), () -> 1);
+
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      scheduler.schedule(runWhenDisabled, dontRunWhenDisabled);
+
+      assertTrue(scheduler.isScheduled(runWhenDisabled));
+      assertFalse(scheduler.isScheduled(dontRunWhenDisabled));
+    }
+  }
+
+  @Test
+  void parallelConditionalRunWhenDisabledTest() {
+    setDSEnabled(false);
+
+    MockCommandHolder command1Holder = new MockCommandHolder(true);
+    Command command1 = command1Holder.getMock();
+    MockCommandHolder command2Holder = new MockCommandHolder(true);
+    Command command2 = command2Holder.getMock();
+    MockCommandHolder command3Holder = new MockCommandHolder(true);
+    Command command3 = command3Holder.getMock();
+    MockCommandHolder command4Holder = new MockCommandHolder(false);
+    Command command4 = command4Holder.getMock();
+
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      Command runWhenDisabled = new ConditionalCommand(command1, command2, () -> true);
+      Command dontRunWhenDisabled = new ConditionalCommand(command3, command4, () -> true);
+
+      Command parallel = parallel(runWhenDisabled, dontRunWhenDisabled);
+
+      scheduler.schedule(parallel);
+
+      assertFalse(scheduler.isScheduled(runWhenDisabled));
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.Test;
+
+class RunCommandTest extends CommandTestBase {
+  @Test
+  void runCommandScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger(0);
+
+      RunCommand command = new RunCommand(counter::incrementAndGet);
+
+      scheduler.schedule(command);
+      scheduler.run();
+      scheduler.run();
+      scheduler.run();
+
+      assertEquals(3, counter.get());
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java

@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.mockito.Mockito.verify;
+
+import org.junit.jupiter.api.Test;
+
+class ScheduleCommandTest extends CommandTestBase {
+  @Test
+  void scheduleCommandScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+
+      ScheduleCommand scheduleCommand = new ScheduleCommand(command1, command2);
+
+      scheduler.schedule(scheduleCommand);
+
+      verify(command1).initialize();
+      verify(command2).initialize();
+    }
+  }
+
+  @Test
+  void scheduleCommandDuringRunTest() {
+    try (CommandScheduler scheduler = CommandScheduler.getInstance()) {
+      Command toSchedule = Commands.none();
+      ScheduleCommand scheduleCommand = new ScheduleCommand(toSchedule);
+      Command group = Commands.sequence(Commands.none(), scheduleCommand);
+
+      scheduler.schedule(group);
+      scheduler.schedule(Commands.idle());
+      scheduler.run();
+      assertDoesNotThrow(scheduler::run);
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java

@@ -0,0 +1,464 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+class SchedulingRecursionTest extends CommandTestBase {
+  /**
+   * <a href="https://github.com/wpilibsuite/allwpilib/issues/4259">wpilibsuite/allwpilib#4259</a>.
+   */
+  @EnumSource(InterruptionBehavior.class)
+  @ParameterizedTest
+  void cancelFromInitialize(InterruptionBehavior interruptionBehavior) {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean hasOtherRun = new AtomicBoolean();
+      AtomicInteger counter = new AtomicInteger();
+      Subsystem requirement = new SubsystemBase() {};
+      Command selfCancels =
+          new Command() {
+            {
+              addRequirements(requirement);
+            }
+
+            @Override
+            public void initialize() {
+              scheduler.cancel(this);
+            }
+
+            @Override
+            public void end(boolean interrupted) {
+              counter.incrementAndGet();
+            }
+
+            @Override
+            public InterruptionBehavior getInterruptionBehavior() {
+              return interruptionBehavior;
+            }
+          };
+      Command other = requirement.run(() -> hasOtherRun.set(true));
+
+      assertDoesNotThrow(
+          () -> {
+            scheduler.schedule(selfCancels);
+            scheduler.run();
+            scheduler.schedule(other);
+          });
+      assertFalse(scheduler.isScheduled(selfCancels));
+      assertTrue(scheduler.isScheduled(other));
+      assertEquals(1, counter.get());
+      scheduler.run();
+      assertTrue(hasOtherRun.get());
+    }
+  }
+
+  @EnumSource(InterruptionBehavior.class)
+  @ParameterizedTest
+  void cancelFromInitializeAction(InterruptionBehavior interruptionBehavior) {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean hasOtherRun = new AtomicBoolean();
+      AtomicInteger counter = new AtomicInteger();
+      Subsystem requirement = new Subsystem() {};
+      Command selfCancels =
+          new Command() {
+            {
+              addRequirements(requirement);
+            }
+
+            @Override
+            public void end(boolean interrupted) {
+              counter.incrementAndGet();
+            }
+
+            @Override
+            public InterruptionBehavior getInterruptionBehavior() {
+              return interruptionBehavior;
+            }
+          };
+      Command other = requirement.run(() -> hasOtherRun.set(true));
+
+      assertDoesNotThrow(
+          () -> {
+            scheduler.onCommandInitialize(cmd -> scheduler.cancel(selfCancels));
+            scheduler.schedule(selfCancels);
+            scheduler.run();
+            scheduler.schedule(other);
+          });
+      assertFalse(scheduler.isScheduled(selfCancels));
+      assertTrue(scheduler.isScheduled(other));
+      assertEquals(1, counter.get());
+      scheduler.run();
+      assertTrue(hasOtherRun.get());
+    }
+  }
+
+  @EnumSource(InterruptionBehavior.class)
+  @ParameterizedTest
+  void defaultCommandGetsRescheduledAfterSelfCanceling(InterruptionBehavior interruptionBehavior) {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean hasOtherRun = new AtomicBoolean();
+      AtomicInteger counter = new AtomicInteger();
+      Subsystem requirement = new SubsystemBase() {};
+      Command selfCancels =
+          new Command() {
+            {
+              addRequirements(requirement);
+            }
+
+            @Override
+            public void initialize() {
+              scheduler.cancel(this);
+            }
+
+            @Override
+            public void end(boolean interrupted) {
+              counter.incrementAndGet();
+            }
+
+            @Override
+            public InterruptionBehavior getInterruptionBehavior() {
+              return interruptionBehavior;
+            }
+          };
+      Command other = requirement.run(() -> hasOtherRun.set(true));
+      scheduler.setDefaultCommand(requirement, other);
+
+      assertDoesNotThrow(
+          () -> {
+            scheduler.schedule(selfCancels);
+            scheduler.run();
+          });
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(selfCancels));
+      assertTrue(scheduler.isScheduled(other));
+      assertEquals(1, counter.get());
+      scheduler.run();
+      assertTrue(hasOtherRun.get());
+    }
+  }
+
+  @Test
+  void cancelFromEnd() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+      Command selfCancels =
+          new Command() {
+            @Override
+            public void end(boolean interrupted) {
+              counter.incrementAndGet();
+              scheduler.cancel(this);
+            }
+          };
+      scheduler.schedule(selfCancels);
+
+      assertDoesNotThrow(() -> scheduler.cancel(selfCancels));
+      assertEquals(1, counter.get());
+      assertFalse(scheduler.isScheduled(selfCancels));
+    }
+  }
+
+  @Test
+  void cancelFromInterruptAction() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+      Command selfCancels = Commands.idle();
+      scheduler.onCommandInterrupt(
+          cmd -> {
+            counter.incrementAndGet();
+            scheduler.cancel(selfCancels);
+          });
+      scheduler.schedule(selfCancels);
+
+      assertDoesNotThrow(() -> scheduler.cancel(selfCancels));
+      assertEquals(1, counter.get());
+      assertFalse(scheduler.isScheduled(selfCancels));
+    }
+  }
+
+  @Test
+  void cancelFromEndLoop() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+      FunctionalCommand dCancelsAll =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.cancelAll();
+              },
+              () -> true);
+      FunctionalCommand cCancelsD =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.cancel(dCancelsAll);
+              },
+              () -> true);
+      FunctionalCommand bCancelsC =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.cancel(cCancelsD);
+              },
+              () -> true);
+      FunctionalCommand aCancelsB =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.cancel(bCancelsC);
+              },
+              () -> true);
+
+      scheduler.schedule(aCancelsB);
+      scheduler.schedule(bCancelsC);
+      scheduler.schedule(cCancelsD);
+      scheduler.schedule(dCancelsAll);
+
+      assertDoesNotThrow(() -> scheduler.cancel(aCancelsB));
+      assertEquals(4, counter.get());
+      assertFalse(scheduler.isScheduled(aCancelsB));
+      assertFalse(scheduler.isScheduled(bCancelsC));
+      assertFalse(scheduler.isScheduled(cCancelsD));
+      assertFalse(scheduler.isScheduled(dCancelsAll));
+    }
+  }
+
+  @Test
+  void cancelFromEndLoopWhileInRunLoop() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+      FunctionalCommand dCancelsAll =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.cancelAll();
+              },
+              () -> true);
+      FunctionalCommand cCancelsD =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.cancel(dCancelsAll);
+              },
+              () -> true);
+      FunctionalCommand bCancelsC =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.cancel(cCancelsD);
+              },
+              () -> true);
+      FunctionalCommand aCancelsB =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.cancel(bCancelsC);
+              },
+              () -> true);
+
+      scheduler.schedule(aCancelsB);
+      scheduler.schedule(bCancelsC);
+      scheduler.schedule(cCancelsD);
+      scheduler.schedule(dCancelsAll);
+
+      assertDoesNotThrow(scheduler::run);
+      assertEquals(4, counter.get());
+      assertFalse(scheduler.isScheduled(aCancelsB));
+      assertFalse(scheduler.isScheduled(bCancelsC));
+      assertFalse(scheduler.isScheduled(cCancelsD));
+      assertFalse(scheduler.isScheduled(dCancelsAll));
+    }
+  }
+
+  @Test
+  void multiCancelFromEnd() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+      FunctionalCommand bIncrementsCounter =
+          new FunctionalCommand(
+              () -> {}, () -> {}, interrupted -> counter.incrementAndGet(), () -> true);
+      Command aCancelsB =
+          new Command() {
+            @Override
+            public void end(boolean interrupted) {
+              counter.incrementAndGet();
+              scheduler.cancel(bIncrementsCounter);
+              scheduler.cancel(this);
+            }
+          };
+
+      scheduler.schedule(aCancelsB);
+      scheduler.schedule(bIncrementsCounter);
+
+      assertDoesNotThrow(() -> scheduler.cancel(aCancelsB));
+      assertEquals(2, counter.get());
+      assertFalse(scheduler.isScheduled(aCancelsB));
+      assertFalse(scheduler.isScheduled(bIncrementsCounter));
+    }
+  }
+
+  @Test
+  void scheduleFromEndCancel() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+      Subsystem requirement = new SubsystemBase() {};
+      Command other = requirement.runOnce(() -> {});
+      FunctionalCommand selfCancels =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.schedule(other);
+              },
+              () -> false,
+              requirement);
+
+      scheduler.schedule(selfCancels);
+
+      assertDoesNotThrow(() -> scheduler.cancel(selfCancels));
+      assertEquals(1, counter.get());
+      assertFalse(scheduler.isScheduled(selfCancels));
+    }
+  }
+
+  @Test
+  void scheduleFromEndInterrupt() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+      Subsystem requirement = new SubsystemBase() {};
+      Command other = requirement.runOnce(() -> {});
+      FunctionalCommand selfCancels =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.schedule(other);
+              },
+              () -> false,
+              requirement);
+
+      scheduler.schedule(selfCancels);
+
+      assertDoesNotThrow(() -> scheduler.schedule(other));
+      assertEquals(1, counter.get());
+      assertFalse(scheduler.isScheduled(selfCancels));
+      assertTrue(scheduler.isScheduled(other));
+    }
+  }
+
+  @Test
+  void scheduleFromEndInterruptAction() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+      Subsystem requirement = new Subsystem() {};
+      Command other = requirement.runOnce(() -> {});
+      Command selfCancels = requirement.runOnce(() -> {});
+      scheduler.onCommandInterrupt(
+          cmd -> {
+            counter.incrementAndGet();
+            scheduler.schedule(other);
+          });
+      scheduler.schedule(selfCancels);
+
+      assertDoesNotThrow(() -> scheduler.schedule(other));
+      assertEquals(1, counter.get());
+      assertFalse(scheduler.isScheduled(selfCancels));
+      assertTrue(scheduler.isScheduled(other));
+    }
+  }
+
+  @ParameterizedTest
+  @EnumSource(InterruptionBehavior.class)
+  void scheduleInitializeFromDefaultCommand(InterruptionBehavior interruptionBehavior) {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+      Subsystem requirement = new SubsystemBase() {};
+      Command other = requirement.runOnce(() -> {}).withInterruptBehavior(interruptionBehavior);
+      FunctionalCommand defaultCommand =
+          new FunctionalCommand(
+              () -> {
+                counter.incrementAndGet();
+                scheduler.schedule(other);
+              },
+              () -> {},
+              interrupted -> {},
+              () -> false,
+              requirement);
+
+      scheduler.setDefaultCommand(requirement, defaultCommand);
+
+      scheduler.run();
+      scheduler.run();
+      scheduler.run();
+      assertEquals(3, counter.get());
+      assertFalse(scheduler.isScheduled(defaultCommand));
+      assertTrue(scheduler.isScheduled(other));
+    }
+  }
+
+  @Test
+  void cancelDefaultCommandFromEnd() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicInteger counter = new AtomicInteger();
+      Subsystem requirement = new Subsystem() {};
+      Command defaultCommand =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> counter.incrementAndGet(),
+              () -> false,
+              requirement);
+      Command other = requirement.runOnce(() -> {});
+      Command cancelDefaultCommand =
+          new FunctionalCommand(
+              () -> {},
+              () -> {},
+              interrupted -> {
+                counter.incrementAndGet();
+                scheduler.schedule(other);
+              },
+              () -> false);
+
+      assertDoesNotThrow(
+          () -> {
+            scheduler.schedule(cancelDefaultCommand);
+            scheduler.setDefaultCommand(requirement, defaultCommand);
+
+            scheduler.run();
+            scheduler.cancel(cancelDefaultCommand);
+          });
+      assertEquals(2, counter.get());
+      assertFalse(scheduler.isScheduled(defaultCommand));
+      assertTrue(scheduler.isScheduled(other));
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java

@@ -0,0 +1,118 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+import java.util.HashMap;
+import java.util.Map;
+import org.junit.jupiter.api.Test;
+
+class SelectCommandTest extends MultiCompositionTestBase<SelectCommand<Integer>> {
+  @Test
+  void selectCommandTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      command1Holder.setFinished(true);
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
+
+      SelectCommand<String> selectCommand =
+          new SelectCommand<>(
+              Map.ofEntries(
+                  Map.entry("one", command1),
+                  Map.entry("two", command2),
+                  Map.entry("three", command3)),
+              () -> "one");
+
+      scheduler.schedule(selectCommand);
+      scheduler.run();
+
+      verify(command1).initialize();
+      verify(command1).execute();
+      verify(command1).end(false);
+
+      verify(command2, never()).initialize();
+      verify(command2, never()).execute();
+      verify(command2, never()).end(false);
+
+      verify(command3, never()).initialize();
+      verify(command3, never()).execute();
+      verify(command3, never()).end(false);
+    }
+  }
+
+  @Test
+  void selectCommandInvalidKeyTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      command1Holder.setFinished(true);
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
+
+      SelectCommand<String> selectCommand =
+          new SelectCommand<>(
+              Map.ofEntries(
+                  Map.entry("one", command1),
+                  Map.entry("two", command2),
+                  Map.entry("three", command3)),
+              () -> "four");
+
+      assertDoesNotThrow(() -> scheduler.schedule(selectCommand));
+    }
+  }
+
+  @Test
+  void selectCommandRequirementTest() {
+    Subsystem system1 = new SubsystemBase() {};
+    Subsystem system2 = new SubsystemBase() {};
+    Subsystem system3 = new SubsystemBase() {};
+    Subsystem system4 = new SubsystemBase() {};
+
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+      Command command3 = command3Holder.getMock();
+
+      SelectCommand<String> selectCommand =
+          new SelectCommand<>(
+              Map.ofEntries(
+                  Map.entry("one", command1),
+                  Map.entry("two", command2),
+                  Map.entry("three", command3)),
+              () -> "one");
+
+      scheduler.schedule(selectCommand);
+      scheduler.schedule(system3.runOnce(() -> {}));
+
+      assertFalse(scheduler.isScheduled(selectCommand));
+
+      verify(command1).end(true);
+      verify(command2, never()).end(true);
+      verify(command3, never()).end(true);
+    }
+  }
+
+  @Override
+  public SelectCommand<Integer> compose(Command... members) {
+    var map = new HashMap<Integer, Command>();
+    for (int i = 0; i < members.length; i++) {
+      map.put(i, members[i]);
+    }
+    return new SelectCommand<>(map, () -> 0);
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java

@@ -0,0 +1,129 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+import org.junit.jupiter.api.Test;
+
+class SequentialCommandGroupTest extends MultiCompositionTestBase<SequentialCommandGroup> {
+  @Test
+  void sequentialGroupScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+
+      Command group = new SequentialCommandGroup(command1, command2);
+
+      scheduler.schedule(group);
+
+      verify(command1).initialize();
+      verify(command2, never()).initialize();
+
+      command1Holder.setFinished(true);
+      scheduler.run();
+
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command2).initialize();
+      verify(command2, never()).execute();
+      verify(command2, never()).end(false);
+
+      command2Holder.setFinished(true);
+      scheduler.run();
+
+      verify(command1).execute();
+      verify(command1).end(false);
+      verify(command2).execute();
+      verify(command2).end(false);
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void sequentialGroupInterruptTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true);
+      Command command3 = command3Holder.getMock();
+
+      Command group = new SequentialCommandGroup(command1, command2, command3);
+
+      scheduler.schedule(group);
+
+      command1Holder.setFinished(true);
+      scheduler.run();
+      scheduler.cancel(group);
+      scheduler.run();
+
+      verify(command1).execute();
+      verify(command1, never()).end(true);
+      verify(command1).end(false);
+      verify(command2, never()).execute();
+      verify(command2).end(true);
+      verify(command2, never()).end(false);
+      verify(command3, never()).initialize();
+      verify(command3, never()).execute();
+      verify(command3, never()).end(true);
+      verify(command3, never()).end(false);
+
+      assertFalse(scheduler.isScheduled(group));
+    }
+  }
+
+  @Test
+  void notScheduledCancelTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true);
+      Command command2 = command2Holder.getMock();
+
+      Command group = new SequentialCommandGroup(command1, command2);
+
+      assertDoesNotThrow(() -> scheduler.cancel(group));
+    }
+  }
+
+  @Test
+  void sequentialGroupRequirementTest() {
+    Subsystem system1 = new SubsystemBase() {};
+    Subsystem system2 = new SubsystemBase() {};
+    Subsystem system3 = new SubsystemBase() {};
+    Subsystem system4 = new SubsystemBase() {};
+
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true, system1, system2);
+      Command command1 = command1Holder.getMock();
+      MockCommandHolder command2Holder = new MockCommandHolder(true, system3);
+      Command command2 = command2Holder.getMock();
+      MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
+      Command command3 = command3Holder.getMock();
+
+      Command group = new SequentialCommandGroup(command1, command2);
+
+      scheduler.schedule(group);
+      scheduler.schedule(command3);
+
+      assertFalse(scheduler.isScheduled(group));
+      assertTrue(scheduler.isScheduled(command3));
+    }
+  }
+
+  @Override
+  public SequentialCommandGroup compose(Command... members) {
+    return new SequentialCommandGroup(members);
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java

@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+import org.junit.jupiter.params.provider.ValueSource;
+
+public abstract class SingleCompositionTestBase<T extends Command> extends CommandTestBase {
+  abstract T composeSingle(Command member);
+
+  @EnumSource(Command.InterruptionBehavior.class)
+  @ParameterizedTest
+  void interruptible(Command.InterruptionBehavior interruptionBehavior) {
+    var command = composeSingle(Commands.idle().withInterruptBehavior(interruptionBehavior));
+    assertEquals(interruptionBehavior, command.getInterruptionBehavior());
+  }
+
+  @ValueSource(booleans = {true, false})
+  @ParameterizedTest
+  void runWhenDisabled(boolean runsWhenDisabled) {
+    var command = composeSingle(Commands.idle().ignoringDisable(runsWhenDisabled));
+    assertEquals(runsWhenDisabled, command.runsWhenDisabled());
+  }
+
+  @Test
+  @SuppressWarnings("NoDiscard")
+  void commandInOtherCompositionTest() {
+    var command = Commands.none();
+    new WrapperCommand(command) {};
+    assertThrows(IllegalArgumentException.class, () -> composeSingle(command));
+  }
+
+  @Test
+  void commandInMultipleCompositionsTest() {
+    var command = Commands.none();
+    composeSingle(command);
+    assertThrows(IllegalArgumentException.class, () -> composeSingle(command));
+  }
+
+  @Test
+  void composeThenScheduleTest() {
+    var command = Commands.none();
+    composeSingle(command);
+    assertThrows(
+        IllegalArgumentException.class, () -> CommandScheduler.getInstance().schedule(command));
+  }
+
+  @Test
+  void scheduleThenComposeTest() {
+    var command = Commands.idle();
+    CommandScheduler.getInstance().schedule(command);
+    assertThrows(IllegalArgumentException.class, () -> composeSingle(command));
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.concurrent.atomic.AtomicBoolean;
+import org.junit.jupiter.api.Test;
+
+class StartEndCommandTest extends CommandTestBase {
+  @Test
+  void startEndCommandScheduleTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean cond1 = new AtomicBoolean();
+      AtomicBoolean cond2 = new AtomicBoolean();
+
+      StartEndCommand command = new StartEndCommand(() -> cond1.set(true), () -> cond2.set(true));
+
+      scheduler.schedule(command);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(command));
+
+      scheduler.cancel(command);
+
+      assertFalse(scheduler.isScheduled(command));
+      assertTrue(cond1.get());
+      assertTrue(cond2.get());
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java

@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.ArgumentMatchers.anyDouble;
+import static org.mockito.ArgumentMatchers.notNull;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+import static org.mockito.Mockito.when;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+class WaitCommandTest extends CommandTestBase {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void waitCommandTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      WaitCommand waitCommand = new WaitCommand(2);
+
+      scheduler.schedule(waitCommand);
+      scheduler.run();
+      SimHooks.stepTiming(1);
+      scheduler.run();
+
+      assertTrue(scheduler.isScheduled(waitCommand));
+
+      SimHooks.stepTiming(2);
+
+      scheduler.run();
+
+      assertFalse(scheduler.isScheduled(waitCommand));
+    }
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void withTimeoutTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      MockCommandHolder command1Holder = new MockCommandHolder(true);
+      Command command1 = command1Holder.getMock();
+      when(command1.withTimeout(anyDouble())).thenCallRealMethod();
+      when(command1.raceWith(notNull())).thenCallRealMethod();
+
+      Command timeout = command1.withTimeout(2);
+
+      scheduler.schedule(timeout);
+      scheduler.run();
+
+      verify(command1).initialize();
+      verify(command1).execute();
+      assertFalse(scheduler.isScheduled(command1));
+      assertTrue(scheduler.isScheduled(timeout));
+
+      SimHooks.stepTiming(3);
+      scheduler.run();
+
+      verify(command1).end(true);
+      verify(command1, never()).end(false);
+      assertFalse(scheduler.isScheduled(timeout));
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.concurrent.atomic.AtomicBoolean;
+import org.junit.jupiter.api.Test;
+
+class WaitUntilCommandTest extends CommandTestBase {
+  @Test
+  void waitUntilTest() {
+    try (CommandScheduler scheduler = new CommandScheduler()) {
+      AtomicBoolean condition = new AtomicBoolean();
+
+      Command command = new WaitUntilCommand(condition::get);
+
+      scheduler.schedule(command);
+      scheduler.run();
+      assertTrue(scheduler.isScheduled(command));
+      condition.set(true);
+      scheduler.run();
+      assertFalse(scheduler.isScheduled(command));
+    }
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.CommandTestBase;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class NetworkButtonTest extends CommandTestBase {
+  NetworkTableInstance m_inst;
+
+  @BeforeEach
+  void setup() {
+    m_inst = NetworkTableInstance.create();
+    m_inst.startLocal();
+  }
+
+  @AfterEach
+  void teardown() {
+    m_inst.close();
+  }
+
+  @Test
+  void setNetworkButtonTest() {
+    var scheduler = CommandScheduler.getInstance();
+    var commandHolder = new MockCommandHolder(true);
+    var command = commandHolder.getMock();
+    var pub = m_inst.getTable("TestTable").getBooleanTopic("Test").publish();
+
+    var button = new NetworkButton(m_inst, "TestTable", "Test");
+    pub.set(false);
+    button.onTrue(command);
+    scheduler.run();
+    verify(command, never()).initialize();
+    pub.set(true);
+    scheduler.run();
+    scheduler.run();
+    verify(command).initialize();
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java

@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj2.command.CommandTestBase;
+import org.junit.jupiter.api.Test;
+
+class RobotModeTriggersTest extends CommandTestBase {
+  @Test
+  void autonomousTest() {
+    DriverStationSim.resetData();
+    DriverStationSim.setAutonomous(true);
+    DriverStationSim.setTest(false);
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.notifyNewData();
+    Trigger auto = RobotModeTriggers.autonomous();
+    assertTrue(auto.getAsBoolean());
+  }
+
+  @Test
+  void teleopTest() {
+    DriverStationSim.resetData();
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.notifyNewData();
+    Trigger teleop = RobotModeTriggers.teleop();
+    assertTrue(teleop.getAsBoolean());
+  }
+
+  @Test
+  void testModeTest() {
+    DriverStationSim.resetData();
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(true);
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.notifyNewData();
+    Trigger test = RobotModeTriggers.test();
+    assertTrue(test.getAsBoolean());
+  }
+
+  @Test
+  void disabledTest() {
+    DriverStationSim.resetData();
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.notifyNewData();
+    Trigger disabled = RobotModeTriggers.disabled();
+    assertTrue(disabled.getAsBoolean());
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/button/TriggerTest.java

@@ -0,0 +1,278 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.button;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.CommandTestBase;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.function.BooleanSupplier;
+import org.junit.jupiter.api.Test;
+
+class TriggerTest extends CommandTestBase {
+  @Test
+  void onTrueTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    AtomicBoolean finished = new AtomicBoolean(false);
+    Command command1 = new WaitUntilCommand(finished::get);
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.onTrue(command1);
+    scheduler.run();
+    assertFalse(command1.isScheduled());
+    button.setPressed(true);
+    scheduler.run();
+    assertTrue(command1.isScheduled());
+    finished.set(true);
+    scheduler.run();
+    assertFalse(command1.isScheduled());
+  }
+
+  @Test
+  void onFalseTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    AtomicBoolean finished = new AtomicBoolean(false);
+    Command command1 = new WaitUntilCommand(finished::get);
+
+    InternalButton button = new InternalButton();
+    button.setPressed(true);
+    button.onFalse(command1);
+    scheduler.run();
+    assertFalse(command1.isScheduled());
+    button.setPressed(false);
+    scheduler.run();
+    assertTrue(command1.isScheduled());
+    finished.set(true);
+    scheduler.run();
+    assertFalse(command1.isScheduled());
+  }
+
+  @Test
+  void onChangeTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    AtomicBoolean finished = new AtomicBoolean(false);
+    Command command1 = new WaitUntilCommand(finished::get);
+
+    InternalButton button = new InternalButton();
+    button.setPressed(true);
+    button.onChange(command1);
+    scheduler.run();
+    assertFalse(command1.isScheduled());
+    button.setPressed(false);
+    scheduler.run();
+    assertTrue(command1.isScheduled());
+    finished.set(true);
+    scheduler.run();
+    assertFalse(command1.isScheduled());
+    finished.set(false);
+    button.setPressed(true);
+    scheduler.run();
+    assertTrue(command1.isScheduled());
+    finished.set(true);
+    scheduler.run();
+    assertFalse(command1.isScheduled());
+  }
+
+  @Test
+  void whileTrueRepeatedlyTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    AtomicInteger inits = new AtomicInteger(0);
+    AtomicInteger counter = new AtomicInteger(0);
+    // the repeatedly() here is the point!
+    Command command1 =
+        new FunctionalCommand(
+                inits::incrementAndGet,
+                () -> {},
+                interrupted -> {},
+                () -> counter.incrementAndGet() % 2 == 0)
+            .repeatedly();
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whileTrue(command1);
+    scheduler.run();
+    assertEquals(0, inits.get());
+    button.setPressed(true);
+    scheduler.run();
+    assertEquals(1, inits.get());
+    scheduler.run();
+    assertEquals(1, inits.get());
+    scheduler.run();
+    assertEquals(2, inits.get());
+    button.setPressed(false);
+    scheduler.run();
+    assertEquals(2, inits.get());
+  }
+
+  @Test
+  void whileTrueLambdaRunTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    AtomicInteger counter = new AtomicInteger(0);
+    // the repeatedly() here is the point!
+    Command command1 = new RunCommand(counter::incrementAndGet);
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whileTrue(command1);
+    scheduler.run();
+    assertEquals(0, counter.get());
+    button.setPressed(true);
+    scheduler.run();
+    assertEquals(1, counter.get());
+    scheduler.run();
+    assertEquals(2, counter.get());
+    button.setPressed(false);
+    scheduler.run();
+    assertEquals(2, counter.get());
+  }
+
+  @Test
+  void whileTrueOnceTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    AtomicInteger startCounter = new AtomicInteger(0);
+    AtomicInteger endCounter = new AtomicInteger(0);
+    Command command1 =
+        new StartEndCommand(startCounter::incrementAndGet, endCounter::incrementAndGet);
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.whileTrue(command1);
+    scheduler.run();
+    assertEquals(0, startCounter.get());
+    assertEquals(0, endCounter.get());
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    assertEquals(1, startCounter.get());
+    assertEquals(0, endCounter.get());
+    button.setPressed(false);
+    scheduler.run();
+    assertEquals(1, startCounter.get());
+    assertEquals(1, endCounter.get());
+  }
+
+  @Test
+  void toggleOnTrueTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    AtomicInteger startCounter = new AtomicInteger(0);
+    AtomicInteger endCounter = new AtomicInteger(0);
+    Command command1 =
+        new StartEndCommand(startCounter::incrementAndGet, endCounter::incrementAndGet);
+
+    InternalButton button = new InternalButton();
+    button.setPressed(false);
+    button.toggleOnTrue(command1);
+    scheduler.run();
+    assertEquals(0, startCounter.get());
+    assertEquals(0, endCounter.get());
+    button.setPressed(true);
+    scheduler.run();
+    scheduler.run();
+    assertEquals(1, startCounter.get());
+    assertEquals(0, endCounter.get());
+    button.setPressed(false);
+    scheduler.run();
+    assertEquals(1, startCounter.get());
+    assertEquals(0, endCounter.get());
+    button.setPressed(true);
+    scheduler.run();
+    assertEquals(1, startCounter.get());
+    assertEquals(1, endCounter.get());
+  }
+
+  @Test
+  void cancelWhenActiveTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    AtomicInteger startCounter = new AtomicInteger(0);
+    AtomicInteger endCounter = new AtomicInteger(0);
+
+    InternalButton button = new InternalButton();
+    Command command1 =
+        new StartEndCommand(startCounter::incrementAndGet, endCounter::incrementAndGet)
+            .until(button);
+
+    button.setPressed(false);
+    scheduler.schedule(command1);
+    scheduler.run();
+    assertEquals(1, startCounter.get());
+    assertEquals(0, endCounter.get());
+    button.setPressed(true);
+    scheduler.run();
+    assertEquals(1, startCounter.get());
+    assertEquals(1, endCounter.get());
+    scheduler.run();
+    assertEquals(1, startCounter.get());
+    assertEquals(1, endCounter.get());
+  }
+
+  @Test
+  void triggerCompositionTest() {
+    InternalButton button1 = new InternalButton();
+    InternalButton button2 = new InternalButton();
+
+    button1.setPressed(true);
+    button2.setPressed(false);
+
+    assertFalse(button1.and(button2).getAsBoolean());
+    assertTrue(button1.or(button2).getAsBoolean());
+    assertFalse(button1.negate().getAsBoolean());
+    assertTrue(button1.and(button2.negate()).getAsBoolean());
+  }
+
+  @Test
+  void triggerCompositionSupplierTest() {
+    InternalButton button1 = new InternalButton();
+    BooleanSupplier booleanSupplier = () -> false;
+
+    button1.setPressed(true);
+
+    assertFalse(button1.and(booleanSupplier).getAsBoolean());
+    assertTrue(button1.or(booleanSupplier).getAsBoolean());
+  }
+
+  @Test
+  void debounceTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder commandHolder = new MockCommandHolder(true);
+    Command command = commandHolder.getMock();
+
+    InternalButton button = new InternalButton();
+    Trigger debounced = button.debounce(0.1);
+
+    debounced.onTrue(command);
+
+    button.setPressed(true);
+    scheduler.run();
+    verify(command, never()).initialize();
+
+    SimHooks.stepTiming(0.3);
+
+    button.setPressed(true);
+    scheduler.run();
+    verify(command).initialize();
+  }
+
+  @Test
+  void booleanSupplierTest() {
+    InternalButton button = new InternalButton();
+
+    assertFalse(button.getAsBoolean());
+    button.setPressed(true);
+    assertTrue(button.getAsBoolean());
+  }
+}

commandsv2/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java

@@ -0,0 +1,143 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj2.command.sysid;
+
+import static edu.wpi.first.units.Units.Volts;
+import static org.mockito.ArgumentMatchers.any;
+import static org.mockito.Mockito.atLeastOnce;
+import static org.mockito.Mockito.clearInvocations;
+import static org.mockito.Mockito.inOrder;
+import static org.mockito.Mockito.mock;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class SysIdRoutineTest {
+  interface Mechanism extends Subsystem {
+    void recordState(SysIdRoutineLog.State state);
+
+    void drive(Voltage voltage);
+
+    void log(SysIdRoutineLog log);
+  }
+
+  Mechanism m_mechanism;
+  SysIdRoutine m_sysidRoutine;
+  Command m_quasistaticForward;
+  Command m_quasistaticReverse;
+  Command m_dynamicForward;
+  Command m_dynamicReverse;
+
+  void runCommand(Command command) {
+    command.initialize();
+    command.execute();
+    command.execute();
+    SimHooks.stepTiming(1);
+    command.execute();
+    command.end(true);
+  }
+
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    m_mechanism = mock(Mechanism.class);
+    m_sysidRoutine =
+        new SysIdRoutine(
+            new SysIdRoutine.Config(null, null, null, m_mechanism::recordState),
+            new SysIdRoutine.Mechanism(m_mechanism::drive, m_mechanism::log, new Subsystem() {}));
+    m_quasistaticForward = m_sysidRoutine.quasistatic(SysIdRoutine.Direction.kForward);
+    m_quasistaticReverse = m_sysidRoutine.quasistatic(SysIdRoutine.Direction.kReverse);
+    m_dynamicForward = m_sysidRoutine.dynamic(SysIdRoutine.Direction.kForward);
+    m_dynamicReverse = m_sysidRoutine.dynamic(SysIdRoutine.Direction.kReverse);
+  }
+
+  @AfterEach
+  void cleanupAll() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  void recordStateBookendsMotorLogging() {
+    runCommand(m_quasistaticForward);
+
+    var orderCheck = inOrder(m_mechanism);
+
+    orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kQuasistaticForward);
+    orderCheck.verify(m_mechanism).drive(any());
+    orderCheck.verify(m_mechanism).log(any());
+    orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kNone);
+    orderCheck.verifyNoMoreInteractions();
+
+    clearInvocations(m_mechanism);
+    orderCheck = inOrder(m_mechanism);
+    runCommand(m_dynamicForward);
+
+    orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kDynamicForward);
+    orderCheck.verify(m_mechanism).drive(any());
+    orderCheck.verify(m_mechanism).log(any());
+    orderCheck.verify(m_mechanism).recordState(SysIdRoutineLog.State.kNone);
+    orderCheck.verifyNoMoreInteractions();
+  }
+
+  @Test
+  void testsDeclareCorrectState() {
+    runCommand(m_quasistaticForward);
+    verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kQuasistaticForward);
+
+    runCommand(m_quasistaticReverse);
+    verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kQuasistaticReverse);
+
+    runCommand(m_dynamicForward);
+    verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kDynamicForward);
+
+    runCommand(m_dynamicReverse);
+    verify(m_mechanism, atLeastOnce()).recordState(SysIdRoutineLog.State.kDynamicReverse);
+  }
+
+  @Test
+  void testsOutputCorrectVoltage() {
+    runCommand(m_quasistaticForward);
+    var orderCheck = inOrder(m_mechanism);
+
+    orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(1));
+    orderCheck.verify(m_mechanism).drive(Volts.of(0));
+    orderCheck.verify(m_mechanism, never()).drive(any());
+
+    clearInvocations(m_mechanism);
+    runCommand(m_quasistaticReverse);
+    orderCheck = inOrder(m_mechanism);
+
+    orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(-1));
+    orderCheck.verify(m_mechanism).drive(Volts.of(0));
+    orderCheck.verify(m_mechanism, never()).drive(any());
+
+    clearInvocations(m_mechanism);
+    runCommand(m_dynamicForward);
+    orderCheck = inOrder(m_mechanism);
+
+    orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(7));
+    orderCheck.verify(m_mechanism).drive(Volts.of(0));
+    orderCheck.verify(m_mechanism, never()).drive(any());
+
+    clearInvocations(m_mechanism);
+    runCommand(m_dynamicForward);
+    orderCheck = inOrder(m_mechanism);
+
+    runCommand(m_dynamicReverse);
+    orderCheck.verify(m_mechanism, atLeastOnce()).drive(Volts.of(-7));
+    orderCheck.verify(m_mechanism).drive(Volts.of(0));
+    orderCheck.verify(m_mechanism, never()).drive(any());
+  }
+}

commandsv2/src/test/native/cpp/frc2/command/AddRequirementsTest.cpp

@@ -0,0 +1,144 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <wpi/array.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/Command.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+
+// Class to verify the overload resolution of Command::AddRequirements. This
+// does not derive from Command because AddRequirements is non-virtual,
+// preventing overriding anyways.
+class MockAddRequirements {
+ public:
+  MOCK_METHOD(void, AddRequirements, (Requirements), ());
+  MOCK_METHOD(void, AddRequirements, ((wpi::SmallSet<Subsystem*, 4>)), ());
+  MOCK_METHOD(void, AddRequirements, (Subsystem*), ());
+};
+
+TEST(AddRequirementsTest, InitializerListOverloadResolution) {
+  TestSubsystem requirement;
+
+  MockAddRequirements overloadResolver;
+
+  EXPECT_CALL(overloadResolver, AddRequirements(testing::An<Requirements>()));
+
+  overloadResolver.AddRequirements({&requirement, &requirement});
+}
+
+TEST(AddRequirementsTest, SpanOverloadResolution) {
+  std::span<Subsystem* const> requirementsSpan;
+
+  MockAddRequirements overloadResolver;
+
+  EXPECT_CALL(overloadResolver, AddRequirements(testing::An<Requirements>()));
+
+  overloadResolver.AddRequirements(requirementsSpan);
+}
+
+TEST(AddRequirementsTest, SmallSetOverloadResolution) {
+  wpi::SmallSet<Subsystem*, 4> requirementsSet;
+
+  MockAddRequirements overloadResolver;
+
+  EXPECT_CALL(overloadResolver,
+              AddRequirements(testing::An<wpi::SmallSet<Subsystem*, 4>>()));
+
+  overloadResolver.AddRequirements(requirementsSet);
+}
+
+TEST(AddRequirementsTest, SubsystemOverloadResolution) {
+  TestSubsystem requirement;
+
+  MockAddRequirements overloadResolver;
+
+  EXPECT_CALL(overloadResolver, AddRequirements(testing::An<Subsystem*>()));
+
+  overloadResolver.AddRequirements(&requirement);
+}
+
+TEST(AddRequirementsTest, InitializerListSemantics) {
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+
+  RunCommand command([] {});
+  command.AddRequirements({&requirement1, &requirement2});
+  EXPECT_TRUE(command.HasRequirement(&requirement1));
+  EXPECT_TRUE(command.HasRequirement(&requirement2));
+  EXPECT_EQ(command.GetRequirements().size(), 2u);
+}
+
+TEST(AddRequirementsTest, InitializerListDuplicatesSemantics) {
+  TestSubsystem requirement;
+
+  RunCommand command([] {});
+  command.AddRequirements({&requirement, &requirement});
+  EXPECT_TRUE(command.HasRequirement(&requirement));
+  EXPECT_EQ(command.GetRequirements().size(), 1u);
+}
+
+TEST(AddRequirementsTest, SpanSemantics) {
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+
+  wpi::array<Subsystem* const, 2> requirementsArray(&requirement1,
+                                                    &requirement2);
+
+  RunCommand command([] {});
+  command.AddRequirements(std::span{requirementsArray});
+  EXPECT_TRUE(command.HasRequirement(&requirement1));
+  EXPECT_TRUE(command.HasRequirement(&requirement2));
+  EXPECT_EQ(command.GetRequirements().size(), 2u);
+}
+
+TEST(AddRequirementsTest, SpanDuplicatesSemantics) {
+  TestSubsystem requirement;
+
+  wpi::array<Subsystem* const, 2> requirementsArray(&requirement, &requirement);
+
+  RunCommand command([] {});
+  command.AddRequirements(std::span{requirementsArray});
+  EXPECT_TRUE(command.HasRequirement(&requirement));
+  EXPECT_EQ(command.GetRequirements().size(), 1u);
+}
+
+TEST(AddRequirementsTest, SmallSetSemantics) {
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+
+  wpi::SmallSet<Subsystem*, 4> requirementsSet;
+  requirementsSet.insert(&requirement1);
+  requirementsSet.insert(&requirement2);
+
+  RunCommand command([] {});
+  command.AddRequirements(requirementsSet);
+  EXPECT_TRUE(command.HasRequirement(&requirement1));
+  EXPECT_TRUE(command.HasRequirement(&requirement2));
+  EXPECT_EQ(command.GetRequirements().size(), 2u);
+}
+
+TEST(AddRequirementsTest, SubsystemPointerSemantics) {
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+
+  RunCommand command([] {});
+  command.AddRequirements(&requirement1);
+  command.AddRequirements(&requirement2);
+  EXPECT_TRUE(command.HasRequirement(&requirement1));
+  EXPECT_TRUE(command.HasRequirement(&requirement2));
+  EXPECT_EQ(command.GetRequirements().size(), 2u);
+}
+
+TEST(AddRequirementsTest, SubsystemPointerDuplicatesSemantics) {
+  TestSubsystem requirement;
+
+  RunCommand command([] {});
+  command.AddRequirements(&requirement);
+  command.AddRequirements(&requirement);
+  EXPECT_TRUE(command.HasRequirement(&requirement));
+  EXPECT_EQ(command.GetRequirements().size(), 1u);
+}

commandsv2/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp

@@ -0,0 +1,491 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <string>
+#include <utility>
+
+#include <frc/simulation/SimHooks.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/Commands.h"
+#include "frc2/command/FunctionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class CommandDecoratorTest : public CommandTestBase {};
+
+TEST_F(CommandDecoratorTest, WithTimeout) {
+  CommandScheduler scheduler = GetScheduler();
+
+  frc::sim::PauseTiming();
+
+  auto command = cmd::Idle().WithTimeout(100_ms);
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(command));
+
+  frc::sim::StepTiming(150_ms);
+
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(command));
+
+  frc::sim::ResumeTiming();
+}
+
+TEST_F(CommandDecoratorTest, Until) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finish = false;
+
+  auto command = cmd::Idle().Until([&finish] { return finish; });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(command));
+
+  finish = true;
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(command));
+}
+
+TEST_F(CommandDecoratorTest, UntilOrder) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool firstHasRun = false;
+  bool firstWasPolled = false;
+
+  auto first = FunctionalCommand([] {}, [&firstHasRun] { firstHasRun = true; },
+                                 [](bool interrupted) {},
+                                 [&firstWasPolled] {
+                                   firstWasPolled = true;
+                                   return true;
+                                 });
+  auto command = std::move(first).Until([&firstHasRun, &firstWasPolled] {
+    EXPECT_TRUE(firstHasRun);
+    EXPECT_TRUE(firstWasPolled);
+    return true;
+  });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+
+  EXPECT_TRUE(firstHasRun);
+  EXPECT_TRUE(firstWasPolled);
+}
+
+TEST_F(CommandDecoratorTest, OnlyWhile) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool run = true;
+
+  auto command = cmd::Idle().OnlyWhile([&run] { return run; });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(command));
+
+  run = false;
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(command));
+}
+
+TEST_F(CommandDecoratorTest, OnlyWhileOrder) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool firstHasRun = false;
+  bool firstWasPolled = false;
+
+  auto first = FunctionalCommand([] {}, [&firstHasRun] { firstHasRun = true; },
+                                 [](bool interrupted) {},
+                                 [&firstWasPolled] {
+                                   firstWasPolled = true;
+                                   return true;
+                                 });
+  auto command = std::move(first).Until([&firstHasRun, &firstWasPolled] {
+    EXPECT_TRUE(firstHasRun);
+    EXPECT_TRUE(firstWasPolled);
+    return false;
+  });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+
+  EXPECT_TRUE(firstHasRun);
+  EXPECT_TRUE(firstWasPolled);
+}
+
+TEST_F(CommandDecoratorTest, IgnoringDisable) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = cmd::Idle().IgnoringDisable(true);
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(command);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(command));
+}
+
+TEST_F(CommandDecoratorTest, BeforeStarting) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command = cmd::None().BeforeStarting([&finished] { finished = true; });
+
+  scheduler.Schedule(command);
+
+  EXPECT_TRUE(finished);
+
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(command));
+
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(command));
+}
+
+TEST_F(CommandDecoratorTest, AndThenLambda) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command = cmd::None().AndThen([&finished] { finished = true; });
+
+  scheduler.Schedule(command);
+
+  EXPECT_FALSE(finished);
+
+  scheduler.Run();
+
+  EXPECT_TRUE(finished);
+
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(command));
+}
+
+TEST_F(CommandDecoratorTest, AndThen) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command1 = cmd::None();
+  auto command2 = cmd::RunOnce([&finished] { finished = true; });
+  auto group = std::move(command1).AndThen(std::move(command2));
+
+  scheduler.Schedule(group);
+
+  EXPECT_FALSE(finished);
+
+  scheduler.Run();
+
+  EXPECT_TRUE(finished);
+
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+TEST_F(CommandDecoratorTest, DeadlineFor) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finish = false;
+
+  auto dictator = cmd::WaitUntil([&finish] { return finish; });
+  auto endsAfter = cmd::Idle();
+
+  auto group = std::move(dictator).DeadlineFor(std::move(endsAfter));
+
+  scheduler.Schedule(group);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(group));
+
+  finish = true;
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+TEST_F(CommandDecoratorTest, WithDeadline) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finish = false;
+
+  auto dictator = WaitUntilCommand([&finish] { return finish; });
+  auto endsAfter = WaitUntilCommand([] { return false; });
+
+  auto group = std::move(endsAfter).WithDeadline(std::move(dictator).ToPtr());
+
+  scheduler.Schedule(group);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(group));
+
+  finish = true;
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+TEST_F(CommandDecoratorTest, AlongWith) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finish = false;
+
+  auto command1 = cmd::WaitUntil([&finish] { return finish; });
+  auto command2 = cmd::None();
+
+  auto group = std::move(command1).AlongWith(std::move(command2));
+
+  scheduler.Schedule(group);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(group));
+
+  finish = true;
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+TEST_F(CommandDecoratorTest, RaceWith) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command1 = cmd::Idle();
+  auto command2 = cmd::None();
+
+  auto group = std::move(command1).RaceWith(std::move(command2));
+
+  scheduler.Schedule(group);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+TEST_F(CommandDecoratorTest, DeadlineForOrder) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool dictatorHasRun = false;
+  bool dictatorWasPolled = false;
+
+  auto dictator =
+      FunctionalCommand([] {}, [&dictatorHasRun] { dictatorHasRun = true; },
+                        [](bool interrupted) {},
+                        [&dictatorWasPolled] {
+                          dictatorWasPolled = true;
+                          return true;
+                        });
+  auto other = cmd::Run([&dictatorHasRun, &dictatorWasPolled] {
+    EXPECT_TRUE(dictatorHasRun);
+    EXPECT_TRUE(dictatorWasPolled);
+  });
+
+  auto group = std::move(dictator).DeadlineFor(std::move(other));
+
+  scheduler.Schedule(group);
+  scheduler.Run();
+
+  EXPECT_TRUE(dictatorHasRun);
+  EXPECT_TRUE(dictatorWasPolled);
+}
+
+TEST_F(CommandDecoratorTest, WithDeadlineOrder) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool dictatorHasRun = false;
+  bool dictatorWasPolled = false;
+
+  auto dictator =
+      FunctionalCommand([] {}, [&dictatorHasRun] { dictatorHasRun = true; },
+                        [](bool interrupted) {},
+                        [&dictatorWasPolled] {
+                          dictatorWasPolled = true;
+                          return true;
+                        });
+  auto other = RunCommand([&dictatorHasRun, &dictatorWasPolled] {
+    EXPECT_TRUE(dictatorHasRun);
+    EXPECT_TRUE(dictatorWasPolled);
+  });
+
+  auto group = std::move(other).WithDeadline(std::move(dictator).ToPtr());
+
+  scheduler.Schedule(group);
+  scheduler.Run();
+
+  EXPECT_TRUE(dictatorHasRun);
+  EXPECT_TRUE(dictatorWasPolled);
+}
+
+TEST_F(CommandDecoratorTest, AlongWithOrder) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool firstHasRun = false;
+  bool firstWasPolled = false;
+
+  auto command1 = FunctionalCommand(
+      [] {}, [&firstHasRun] { firstHasRun = true; }, [](bool interrupted) {},
+      [&firstWasPolled] {
+        firstWasPolled = true;
+        return true;
+      });
+  auto command2 = cmd::Run([&firstHasRun, &firstWasPolled] {
+    EXPECT_TRUE(firstHasRun);
+    EXPECT_TRUE(firstWasPolled);
+  });
+
+  auto group = std::move(command1).AlongWith(std::move(command2));
+
+  scheduler.Schedule(group);
+  scheduler.Run();
+
+  EXPECT_TRUE(firstHasRun);
+  EXPECT_TRUE(firstWasPolled);
+}
+
+TEST_F(CommandDecoratorTest, RaceWithOrder) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool firstHasRun = false;
+  bool firstWasPolled = false;
+
+  auto command1 = FunctionalCommand(
+      [] {}, [&firstHasRun] { firstHasRun = true; }, [](bool interrupted) {},
+      [&firstWasPolled] {
+        firstWasPolled = true;
+        return true;
+      });
+  auto command2 = cmd::Run([&firstHasRun, &firstWasPolled] {
+    EXPECT_TRUE(firstHasRun);
+    EXPECT_TRUE(firstWasPolled);
+  });
+
+  auto group = std::move(command1).RaceWith(std::move(command2));
+
+  scheduler.Schedule(group);
+  scheduler.Run();
+
+  EXPECT_TRUE(firstHasRun);
+  EXPECT_TRUE(firstWasPolled);
+}
+
+TEST_F(CommandDecoratorTest, Unless) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool hasRun = false;
+  bool unlessCondition = true;
+
+  auto command =
+      cmd::RunOnce([&hasRun] { hasRun = true; }, {}).Unless([&unlessCondition] {
+        return unlessCondition;
+      });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+  EXPECT_FALSE(hasRun);
+
+  unlessCondition = false;
+  scheduler.Schedule(command);
+  scheduler.Run();
+  EXPECT_TRUE(hasRun);
+}
+
+TEST_F(CommandDecoratorTest, OnlyIf) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool hasRun = false;
+  bool onlyIfCondition = false;
+
+  auto command =
+      cmd::RunOnce([&hasRun] { hasRun = true; }, {}).OnlyIf([&onlyIfCondition] {
+        return onlyIfCondition;
+      });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+  EXPECT_FALSE(hasRun);
+
+  onlyIfCondition = true;
+  scheduler.Schedule(command);
+  scheduler.Run();
+  EXPECT_TRUE(hasRun);
+}
+
+TEST_F(CommandDecoratorTest, FinallyDo) {
+  CommandScheduler scheduler = GetScheduler();
+  int first = 0;
+  int second = 0;
+  CommandPtr command = FunctionalCommand([] {}, [] {},
+                                         [&first](bool interrupted) {
+                                           if (!interrupted) {
+                                             first++;
+                                           }
+                                         },
+                                         [] { return true; })
+                           .FinallyDo([&first, &second](bool interrupted) {
+                             if (!interrupted) {
+                               // to differentiate between "didn't run" and "ran
+                               // before command's `end()`
+                               second += 1 + first;
+                             }
+                           });
+
+  scheduler.Schedule(command);
+  EXPECT_EQ(0, first);
+  EXPECT_EQ(0, second);
+  scheduler.Run();
+  EXPECT_EQ(1, first);
+  // if `second == 0`, neither of the lambdas ran.
+  // if `second == 1`, the second lambda ran before the first one
+  EXPECT_EQ(2, second);
+}
+
+// handleInterruptTest() implicitly tests the interrupt=true branch of
+// finallyDo()
+TEST_F(CommandDecoratorTest, HandleInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+  int first = 0;
+  int second = 0;
+  CommandPtr command = FunctionalCommand([] {}, [] {},
+                                         [&first](bool interrupted) {
+                                           if (interrupted) {
+                                             first++;
+                                           }
+                                         },
+                                         [] { return false; })
+                           .HandleInterrupt([&first, &second] {
+                             // to differentiate between "didn't run" and "ran
+                             // before command's `end()`
+                             second += 1 + first;
+                           });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+  EXPECT_EQ(0, first);
+  EXPECT_EQ(0, second);
+
+  scheduler.Cancel(command);
+  // if `second == 0`, neither of the lambdas ran.
+  // if `second == 1`, the second lambda ran before the first one
+  EXPECT_EQ(2, second);
+}
+
+TEST_F(CommandDecoratorTest, WithName) {
+  auto command = cmd::None();
+  std::string name{"Named"};
+  auto named = std::move(command).WithName(name);
+  EXPECT_EQ(name, named.get()->GetName());
+}

commandsv2/src/test/native/cpp/frc2/command/CommandPtrTest.cpp

@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <utility>
+
+#include <frc/Errors.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/Commands.h"
+
+using namespace frc2;
+class CommandPtrTest : public CommandTestBase {};
+
+TEST_F(CommandPtrTest, MovedFrom) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  CommandPtr movedFrom = cmd::Run([&counter] { counter++; });
+  CommandPtr movedTo = std::move(movedFrom);
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Schedule(movedTo));
+  EXPECT_NO_FATAL_FAILURE(scheduler.Run());
+
+  EXPECT_EQ(1, counter);
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(movedTo));
+
+  EXPECT_THROW(scheduler.Schedule(movedFrom), frc::RuntimeError);
+  // NOLINTNEXTLINE(clang-analyzer-cplusplus.Move)
+  EXPECT_THROW(movedFrom.IsScheduled(), frc::RuntimeError);
+  EXPECT_THROW(static_cast<void>(std::move(movedFrom).Repeatedly()),
+               frc::RuntimeError);
+
+  EXPECT_EQ(1, counter);
+}
+
+TEST_F(CommandPtrTest, NullInitialization) {
+  EXPECT_THROW(auto cmd = CommandPtr{std::unique_ptr<Command>{}},
+               frc::RuntimeError);
+}

commandsv2/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp

@@ -0,0 +1,86 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <utility>
+
+#include <frc/Errors.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+class CommandRequirementsTest : public CommandTestBase {};
+
+TEST_F(CommandRequirementsTest, RequirementInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement;
+
+  MockCommand command1({&requirement});
+  MockCommand command2({&requirement});
+
+  EXPECT_CALL(command1, Initialize());
+  EXPECT_CALL(command1, Execute());
+  EXPECT_CALL(command1, End(true));
+  EXPECT_CALL(command1, End(false)).Times(0);
+
+  EXPECT_CALL(command2, Initialize());
+  EXPECT_CALL(command2, Execute());
+  EXPECT_CALL(command2, End(true)).Times(0);
+  EXPECT_CALL(command2, End(false)).Times(0);
+
+  scheduler.Schedule(&command1);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  scheduler.Schedule(&command2);
+  EXPECT_FALSE(scheduler.IsScheduled(&command1));
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+  scheduler.Run();
+}
+
+TEST_F(CommandRequirementsTest, RequirementUninterruptible) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement;
+
+  int initCounter = 0;
+  int exeCounter = 0;
+  int endCounter = 0;
+
+  CommandPtr command1 =
+      FunctionalCommand([&initCounter] { initCounter++; },
+                        [&exeCounter] { exeCounter++; },
+                        [&endCounter](bool interruptible) { endCounter++; },
+                        [] { return false; }, {&requirement})
+          .WithInterruptBehavior(
+              Command::InterruptionBehavior::kCancelIncoming);
+  MockCommand command2({&requirement});
+
+  EXPECT_CALL(command2, Initialize()).Times(0);
+  EXPECT_CALL(command2, Execute()).Times(0);
+  EXPECT_CALL(command2, End(true)).Times(0);
+  EXPECT_CALL(command2, End(false)).Times(0);
+
+  scheduler.Schedule(command1);
+  EXPECT_EQ(1, initCounter);
+  scheduler.Run();
+  EXPECT_EQ(1, exeCounter);
+  EXPECT_TRUE(scheduler.IsScheduled(command1));
+  scheduler.Schedule(&command2);
+  EXPECT_TRUE(scheduler.IsScheduled(command1));
+  EXPECT_FALSE(scheduler.IsScheduled(&command2));
+  scheduler.Run();
+  EXPECT_EQ(2, exeCounter);
+  EXPECT_EQ(0, endCounter);
+}
+
+TEST_F(CommandRequirementsTest, DefaultCommandRequirementError) {
+  TestSubsystem requirement1;
+
+  MockCommand command1;
+
+  ASSERT_THROW(requirement1.SetDefaultCommand(std::move(command1)),
+               frc::RuntimeError);
+}

commandsv2/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp

@@ -0,0 +1,172 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/FunctionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class CommandScheduleTest : public CommandTestBase {};
+
+TEST_F(CommandScheduleTest, InstantSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_CALL(command, Initialize());
+  EXPECT_CALL(command, Execute());
+  EXPECT_CALL(command, End(false));
+
+  command.SetFinished(true);
+  scheduler.Schedule(&command);
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, SingleIterationSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_CALL(command, Initialize());
+  EXPECT_CALL(command, Execute()).Times(2);
+  EXPECT_CALL(command, End(false));
+
+  scheduler.Schedule(&command);
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  scheduler.Run();
+  command.SetFinished(true);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, MultiSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command1;
+  MockCommand command2;
+  MockCommand command3;
+
+  EXPECT_CALL(command1, Initialize());
+  EXPECT_CALL(command1, Execute()).Times(2);
+  EXPECT_CALL(command1, End(false));
+
+  EXPECT_CALL(command2, Initialize());
+  EXPECT_CALL(command2, Execute()).Times(3);
+  EXPECT_CALL(command2, End(false));
+
+  EXPECT_CALL(command3, Initialize());
+  EXPECT_CALL(command3, Execute()).Times(4);
+  EXPECT_CALL(command3, End(false));
+
+  scheduler.Schedule(&command1);
+  scheduler.Schedule(&command2);
+  scheduler.Schedule(&command3);
+  EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
+  command1.SetFinished(true);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled({&command2, &command3}));
+  EXPECT_FALSE(scheduler.IsScheduled(&command1));
+  command2.SetFinished(true);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2}));
+  command3.SetFinished(true);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2, &command3}));
+}
+
+TEST_F(CommandScheduleTest, SchedulerCancel) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_CALL(command, Initialize());
+  EXPECT_CALL(command, Execute());
+  EXPECT_CALL(command, End(false)).Times(0);
+  EXPECT_CALL(command, End(true));
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  scheduler.Cancel(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandScheduleTest, CommandKnowsWhenItEnded) {
+  CommandScheduler scheduler = GetScheduler();
+
+  frc2::FunctionalCommand* commandPtr = nullptr;
+  auto command = frc2::FunctionalCommand(
+      [] {}, [] {},
+      [&](auto isForced) {
+        EXPECT_FALSE(scheduler.IsScheduled(commandPtr))
+            << "Command shouldn't be scheduled when its end is called";
+      },
+      [] { return true; });
+  commandPtr = &command;
+
+  scheduler.Schedule(commandPtr);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(commandPtr))
+      << "Command should be removed from scheduler when its isFinished() "
+         "returns true";
+}
+
+TEST_F(CommandScheduleTest, ScheduleCommandInCommand) {
+  CommandScheduler scheduler = GetScheduler();
+  int counter = 0;
+  frc2::InstantCommand commandToGetScheduled{[&counter] { counter++; }};
+
+  auto command =
+      frc2::RunCommand([&counter, &scheduler, &commandToGetScheduled] {
+        scheduler.Schedule(&commandToGetScheduled);
+        EXPECT_EQ(counter, 1)
+            << "Scheduled command's init was not run immediately "
+               "after getting scheduled";
+      });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_EQ(counter, 1) << "Command 2 was not run when it should have been";
+  EXPECT_TRUE(scheduler.IsScheduled(&commandToGetScheduled))
+      << "Command 2 was not added to scheduler";
+
+  scheduler.Run();
+  EXPECT_EQ(counter, 1) << "Command 2 was run when it shouldn't have been";
+  EXPECT_FALSE(scheduler.IsScheduled(&commandToGetScheduled))
+      << "Command 2 did not end when it should have";
+}
+
+TEST_F(CommandScheduleTest, NotScheduledCancel) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command));
+}
+
+TEST_F(CommandScheduleTest, SmartDashboardCancel) {
+  CommandScheduler scheduler = GetScheduler();
+  frc::SmartDashboard::PutData("Scheduler", &scheduler);
+  frc::SmartDashboard::UpdateValues();
+
+  MockCommand command;
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+  uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(&command);
+  nt::NetworkTableInstance::GetDefault()
+      .GetEntry("/SmartDashboard/Scheduler/Cancel")
+      .SetIntegerArray(
+          std::span<const int64_t>{{static_cast<int64_t>(ptrTmp)}});
+  frc::SmartDashboard::UpdateValues();
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

commandsv2/src/test/native/cpp/frc2/command/CommandSendableButtonTest.cpp

@@ -0,0 +1,98 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc2/command/Commands.h>
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <networktables/BooleanTopic.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "CommandTestBase.h"
+
+using namespace frc2;
+
+class CommandSendableButtonTest : public CommandTestBase {
+ protected:
+  int m_schedule;
+  int m_cancel;
+  nt::BooleanPublisher m_publish;
+  std::optional<CommandPtr> m_command;
+
+  void SetUp() override {
+    m_schedule = 0;
+    m_cancel = 0;
+    m_command = cmd::StartEnd([this] { m_schedule++; }, [this] { m_cancel++; });
+    m_publish = nt::NetworkTableInstance::GetDefault()
+                    .GetBooleanTopic("/SmartDashboard/command/running")
+                    .Publish();
+    frc::SmartDashboard::PutData("command", m_command->get());
+    frc::SmartDashboard::UpdateValues();
+  }
+};
+
+TEST_F(CommandSendableButtonTest, trueAndNotScheduledSchedules) {
+  // Not scheduled and true -> scheduled
+  GetScheduler().Run();
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_FALSE(m_command->IsScheduled());
+  EXPECT_EQ(0, m_schedule);
+  EXPECT_EQ(0, m_cancel);
+
+  m_publish.Set(true);
+  frc::SmartDashboard::UpdateValues();
+  GetScheduler().Run();
+  EXPECT_TRUE(m_command->IsScheduled());
+  EXPECT_EQ(1, m_schedule);
+  EXPECT_EQ(0, m_cancel);
+}
+
+TEST_F(CommandSendableButtonTest, trueAndScheduledNoOp) {
+  // Scheduled and true -> no-op
+  frc2::CommandScheduler::GetInstance().Schedule(m_command.value());
+  GetScheduler().Run();
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_TRUE(m_command->IsScheduled());
+  EXPECT_EQ(1, m_schedule);
+  EXPECT_EQ(0, m_cancel);
+
+  m_publish.Set(true);
+  frc::SmartDashboard::UpdateValues();
+  GetScheduler().Run();
+  EXPECT_TRUE(m_command->IsScheduled());
+  EXPECT_EQ(1, m_schedule);
+  EXPECT_EQ(0, m_cancel);
+}
+
+TEST_F(CommandSendableButtonTest, falseAndNotScheduledNoOp) {
+  // Not scheduled and false -> no-op
+  GetScheduler().Run();
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_FALSE(m_command->IsScheduled());
+  EXPECT_EQ(0, m_schedule);
+  EXPECT_EQ(0, m_cancel);
+
+  m_publish.Set(false);
+  frc::SmartDashboard::UpdateValues();
+  GetScheduler().Run();
+  EXPECT_FALSE(m_command->IsScheduled());
+  EXPECT_EQ(0, m_schedule);
+  EXPECT_EQ(0, m_cancel);
+}
+
+TEST_F(CommandSendableButtonTest, falseAndScheduledCancel) {
+  // Scheduled and false -> cancel
+  frc2::CommandScheduler::GetInstance().Schedule(m_command.value());
+  GetScheduler().Run();
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_TRUE(m_command->IsScheduled());
+  EXPECT_EQ(1, m_schedule);
+  EXPECT_EQ(0, m_cancel);
+
+  m_publish.Set(false);
+  frc::SmartDashboard::UpdateValues();
+  GetScheduler().Run();
+  EXPECT_FALSE(m_command->IsScheduled());
+  EXPECT_EQ(1, m_schedule);
+  EXPECT_EQ(1, m_cancel);
+}

commandsv2/src/test/native/cpp/frc2/command/CommandTestBase.cpp

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "CommandTestBase.h"
+
+using namespace frc2;
+
+CommandTestBase::CommandTestBase() {
+  auto& scheduler = CommandScheduler::GetInstance();
+  scheduler.CancelAll();
+  scheduler.Enable();
+  scheduler.GetActiveButtonLoop()->Clear();
+  scheduler.UnregisterAllSubsystems();
+
+  SetDSEnabled(true);
+}
+
+CommandTestBase::~CommandTestBase() {
+  CommandScheduler::GetInstance().GetActiveButtonLoop()->Clear();
+  CommandScheduler::GetInstance().UnregisterAllSubsystems();
+}
+
+CommandScheduler CommandTestBase::GetScheduler() {
+  return CommandScheduler();
+}
+
+void CommandTestBase::SetDSEnabled(bool enabled) {
+  frc::sim::DriverStationSim::SetDsAttached(true);
+  frc::sim::DriverStationSim::SetEnabled(enabled);
+  frc::sim::DriverStationSim::NotifyNewData();
+}

commandsv2/src/test/native/cpp/frc2/command/CommandTestBase.h

@@ -0,0 +1,129 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <utility>
+
+#include <frc/simulation/DriverStationSim.h>
+#include <gtest/gtest.h>
+
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/Requirements.h"
+#include "frc2/command/SubsystemBase.h"
+#include "gmock/gmock.h"
+
+namespace frc2 {
+
+class TestSubsystem : public SubsystemBase {
+ public:
+  explicit TestSubsystem(std::function<void()> periodic = [] {})
+      : m_periodic{std::move(periodic)} {}
+  void Periodic() override { m_periodic(); }
+
+ private:
+  std::function<void()> m_periodic;
+};
+
+/**
+ * NOTE: Moving mock objects causes EXPECT_CALL to not work correctly!
+ */
+class MockCommand : public CommandHelper<Command, MockCommand> {
+ public:
+  MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet<Subsystem*, 4>());
+  MOCK_METHOD0(IsFinished, bool());
+  MOCK_CONST_METHOD0(RunsWhenDisabled, bool());
+  MOCK_METHOD0(Initialize, void());
+  MOCK_METHOD0(Execute, void());
+  MOCK_METHOD1(End, void(bool interrupted));
+
+  MockCommand() {
+    m_requirements = {};
+    EXPECT_CALL(*this, GetRequirements())
+        .WillRepeatedly(::testing::Return(m_requirements));
+    EXPECT_CALL(*this, IsFinished()).WillRepeatedly(::testing::Return(false));
+    EXPECT_CALL(*this, RunsWhenDisabled())
+        .WillRepeatedly(::testing::Return(true));
+  }
+
+  explicit MockCommand(Requirements requirements, bool finished = false,
+                       bool runWhenDisabled = true) {
+    m_requirements.insert(requirements.begin(), requirements.end());
+    EXPECT_CALL(*this, GetRequirements())
+        .WillRepeatedly(::testing::Return(m_requirements));
+    EXPECT_CALL(*this, IsFinished())
+        .WillRepeatedly(::testing::Return(finished));
+    EXPECT_CALL(*this, RunsWhenDisabled())
+        .WillRepeatedly(::testing::Return(runWhenDisabled));
+  }
+
+  MockCommand(MockCommand&& other) {
+    EXPECT_CALL(*this, IsFinished())
+        .WillRepeatedly(::testing::Return(other.IsFinished()));
+    EXPECT_CALL(*this, RunsWhenDisabled())
+        .WillRepeatedly(::testing::Return(other.RunsWhenDisabled()));
+    std::swap(m_requirements, other.m_requirements);
+    EXPECT_CALL(*this, GetRequirements())
+        .WillRepeatedly(::testing::Return(m_requirements));
+  }
+
+  MockCommand(const MockCommand& other) : CommandHelper{other} {}
+
+  void SetFinished(bool finished) {
+    EXPECT_CALL(*this, IsFinished())
+        .WillRepeatedly(::testing::Return(finished));
+  }
+
+  ~MockCommand() {  // NOLINT
+    auto& scheduler = CommandScheduler::GetInstance();
+    scheduler.Cancel(this);
+  }
+
+ private:
+  wpi::SmallSet<Subsystem*, 4> m_requirements;
+};
+
+class CommandTestBase : public ::testing::Test {
+ public:
+  CommandTestBase();
+
+  ~CommandTestBase() override;
+
+ protected:
+  CommandScheduler GetScheduler();
+
+  void SetDSEnabled(bool enabled);
+};
+
+template <typename T>
+class CommandTestBaseWithParam : public ::testing::TestWithParam<T> {
+ public:
+  CommandTestBaseWithParam() {
+    auto& scheduler = CommandScheduler::GetInstance();
+    scheduler.CancelAll();
+    scheduler.Enable();
+    scheduler.GetActiveButtonLoop()->Clear();
+    scheduler.UnregisterAllSubsystems();
+
+    SetDSEnabled(true);
+  }
+
+  ~CommandTestBaseWithParam() override {
+    CommandScheduler::GetInstance().GetActiveButtonLoop()->Clear();
+    CommandScheduler::GetInstance().UnregisterAllSubsystems();
+  }
+
+ protected:
+  CommandScheduler GetScheduler() { return CommandScheduler(); }
+
+  void SetDSEnabled(bool enabled) {
+    frc::sim::DriverStationSim::SetDsAttached(true);
+    frc::sim::DriverStationSim::SetEnabled(enabled);
+    frc::sim::DriverStationSim::NotifyNewData();
+  }
+};
+
+}  // namespace frc2

commandsv2/src/test/native/cpp/frc2/command/CompositionTestBase.h

@@ -0,0 +1,185 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <utility>
+
+#include <gtest/gtest.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/Commands.h"
+#include "make_vector.h"
+
+namespace frc2 {
+
+inline namespace single {
+template <typename T>
+class SingleCompositionRunsWhenDisabledTest : public CommandTestBase {};
+
+TYPED_TEST_SUITE_P(SingleCompositionRunsWhenDisabledTest);
+
+TYPED_TEST_P(SingleCompositionRunsWhenDisabledTest, True) {
+  auto param = true;
+  TypeParam command = TypeParam(cmd::Idle().IgnoringDisable(param).Unwrap());
+  EXPECT_EQ(param, command.RunsWhenDisabled());
+}
+
+TYPED_TEST_P(SingleCompositionRunsWhenDisabledTest, False) {
+  auto param = false;
+  TypeParam command = TypeParam(cmd::Idle().IgnoringDisable(param).Unwrap());
+  EXPECT_EQ(param, command.RunsWhenDisabled());
+}
+
+REGISTER_TYPED_TEST_SUITE_P(SingleCompositionRunsWhenDisabledTest, True, False);
+
+template <typename T>
+class SingleCompositionInterruptibilityTest : public CommandTestBase {};
+
+TYPED_TEST_SUITE_P(SingleCompositionInterruptibilityTest);
+
+TYPED_TEST_P(SingleCompositionInterruptibilityTest, CancelSelf) {
+  auto param = Command::InterruptionBehavior::kCancelSelf;
+  TypeParam command =
+      TypeParam(cmd::Idle().WithInterruptBehavior(param).Unwrap());
+  EXPECT_EQ(param, command.GetInterruptionBehavior());
+}
+
+TYPED_TEST_P(SingleCompositionInterruptibilityTest, CancelIncoming) {
+  auto param = Command::InterruptionBehavior::kCancelIncoming;
+  TypeParam command =
+      TypeParam(cmd::Idle().WithInterruptBehavior(param).Unwrap());
+  EXPECT_EQ(param, command.GetInterruptionBehavior());
+}
+
+REGISTER_TYPED_TEST_SUITE_P(SingleCompositionInterruptibilityTest, CancelSelf,
+                            CancelIncoming);
+
+#define INSTANTIATE_SINGLE_COMMAND_COMPOSITION_TEST_SUITE(Suite,               \
+                                                          CompositionType)     \
+  INSTANTIATE_TYPED_TEST_SUITE_P(Suite, SingleCompositionInterruptibilityTest, \
+                                 CompositionType);                             \
+  INSTANTIATE_TYPED_TEST_SUITE_P(Suite, SingleCompositionRunsWhenDisabledTest, \
+                                 CompositionType)
+}  // namespace single
+
+inline namespace multi {
+template <typename T>
+class MultiCompositionRunsWhenDisabledTest : public CommandTestBase {};
+
+TYPED_TEST_SUITE_P(MultiCompositionRunsWhenDisabledTest);
+
+TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, OneTrue) {
+  auto param = true;
+  TypeParam command = TypeParam(CommandPtr::UnwrapVector(
+      cmd::impl::MakeVector(cmd::Idle().IgnoringDisable(param))));
+  EXPECT_EQ(param, command.RunsWhenDisabled());
+}
+
+TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, OneFalse) {
+  auto param = false;
+  TypeParam command = TypeParam(CommandPtr::UnwrapVector(
+      cmd::impl::MakeVector(cmd::Idle().IgnoringDisable(param))));
+  EXPECT_EQ(param, command.RunsWhenDisabled());
+}
+
+TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, AllTrue) {
+  TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector(
+      cmd::Idle().IgnoringDisable(true), cmd::Idle().IgnoringDisable(true),
+      cmd::Idle().IgnoringDisable(true))));
+  EXPECT_EQ(true, command.RunsWhenDisabled());
+}
+
+TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, AllFalse) {
+  TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector(
+      cmd::Idle().IgnoringDisable(false), cmd::Idle().IgnoringDisable(false),
+      cmd::Idle().IgnoringDisable(false))));
+  EXPECT_EQ(false, command.RunsWhenDisabled());
+}
+
+TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, TwoTrueOneFalse) {
+  TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector(
+      cmd::Idle().IgnoringDisable(true), cmd::Idle().IgnoringDisable(true),
+      cmd::Idle().IgnoringDisable(false))));
+  EXPECT_EQ(false, command.RunsWhenDisabled());
+}
+
+TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, TwoFalseOneTrue) {
+  TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector(
+      cmd::Idle().IgnoringDisable(false), cmd::Idle().IgnoringDisable(false),
+      cmd::Idle().IgnoringDisable(true))));
+  EXPECT_EQ(false, command.RunsWhenDisabled());
+}
+
+REGISTER_TYPED_TEST_SUITE_P(MultiCompositionRunsWhenDisabledTest, OneTrue,
+                            OneFalse, AllTrue, AllFalse, TwoTrueOneFalse,
+                            TwoFalseOneTrue);
+
+template <typename T>
+class MultiCompositionInterruptibilityTest
+    : public SingleCompositionInterruptibilityTest<T> {};
+
+TYPED_TEST_SUITE_P(MultiCompositionInterruptibilityTest);
+
+TYPED_TEST_P(MultiCompositionInterruptibilityTest, AllCancelSelf) {
+  TypeParam command = TypeParam(CommandPtr::UnwrapVector(
+      cmd::impl::MakeVector(cmd::Idle().WithInterruptBehavior(
+                                Command::InterruptionBehavior::kCancelSelf),
+                            cmd::Idle().WithInterruptBehavior(
+                                Command::InterruptionBehavior::kCancelSelf),
+                            cmd::Idle().WithInterruptBehavior(
+                                Command::InterruptionBehavior::kCancelSelf))));
+  EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf,
+            command.GetInterruptionBehavior());
+}
+
+TYPED_TEST_P(MultiCompositionInterruptibilityTest, AllCancelIncoming) {
+  TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector(
+      cmd::Idle().WithInterruptBehavior(
+          Command::InterruptionBehavior::kCancelIncoming),
+      cmd::Idle().WithInterruptBehavior(
+          Command::InterruptionBehavior::kCancelIncoming),
+      cmd::Idle().WithInterruptBehavior(
+          Command::InterruptionBehavior::kCancelIncoming))));
+  EXPECT_EQ(Command::InterruptionBehavior::kCancelIncoming,
+            command.GetInterruptionBehavior());
+}
+
+TYPED_TEST_P(MultiCompositionInterruptibilityTest, TwoCancelSelfOneIncoming) {
+  TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector(
+      cmd::Idle().WithInterruptBehavior(
+          Command::InterruptionBehavior::kCancelSelf),
+      cmd::Idle().WithInterruptBehavior(
+          Command::InterruptionBehavior::kCancelSelf),
+      cmd::Idle().WithInterruptBehavior(
+          Command::InterruptionBehavior::kCancelIncoming))));
+  EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf,
+            command.GetInterruptionBehavior());
+}
+
+TYPED_TEST_P(MultiCompositionInterruptibilityTest, TwoCancelIncomingOneSelf) {
+  TypeParam command = TypeParam(CommandPtr::UnwrapVector(
+      cmd::impl::MakeVector(cmd::Idle().WithInterruptBehavior(
+                                Command::InterruptionBehavior::kCancelIncoming),
+                            cmd::Idle().WithInterruptBehavior(
+                                Command::InterruptionBehavior::kCancelIncoming),
+                            cmd::Idle().WithInterruptBehavior(
+                                Command::InterruptionBehavior::kCancelSelf))));
+  EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf,
+            command.GetInterruptionBehavior());
+}
+
+REGISTER_TYPED_TEST_SUITE_P(MultiCompositionInterruptibilityTest, AllCancelSelf,
+                            AllCancelIncoming, TwoCancelSelfOneIncoming,
+                            TwoCancelIncomingOneSelf);
+
+#define INSTANTIATE_MULTI_COMMAND_COMPOSITION_TEST_SUITE(Suite,               \
+                                                         CompositionType)     \
+  INSTANTIATE_TYPED_TEST_SUITE_P(Suite, MultiCompositionInterruptibilityTest, \
+                                 CompositionType);                            \
+  INSTANTIATE_TYPED_TEST_SUITE_P(Suite, MultiCompositionRunsWhenDisabledTest, \
+                                 CompositionType)
+}  // namespace multi
+}  // namespace frc2

commandsv2/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp

@@ -0,0 +1,127 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <memory>
+#include <utility>
+
+#include "CommandTestBase.h"
+#include "frc2/command/Commands.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+class ConditionalCommandTest : public CommandTestBase {};
+
+TEST_F(ConditionalCommandTest, ConditionalCommandSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
+  MockCommand* mockptr = mock.get();
+
+  EXPECT_CALL(*mock, Initialize());
+  EXPECT_CALL(*mock, Execute()).Times(2);
+  EXPECT_CALL(*mock, End(false));
+
+  ConditionalCommand conditional(
+      std::move(mock), std::make_unique<InstantCommand>(), [] { return true; });
+
+  scheduler.Schedule(&conditional);
+  scheduler.Run();
+  mockptr->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&conditional));
+}
+
+TEST_F(ConditionalCommandTest, ConditionalCommandRequirement) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ConditionalCommand conditional(std::move(command1), std::move(command2),
+                                 [] { return true; });
+  scheduler.Schedule(&conditional);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&conditional));
+}
+
+TEST_F(ConditionalCommandTest, AllTrue) {
+  auto command =
+      cmd::Either(cmd::Idle().IgnoringDisable(true),
+                  cmd::Idle().IgnoringDisable(true), [] { return true; });
+  EXPECT_EQ(true, command.get()->RunsWhenDisabled());
+}
+
+TEST_F(ConditionalCommandTest, AllFalse) {
+  auto command =
+      cmd::Either(cmd::Idle().IgnoringDisable(false),
+                  cmd::Idle().IgnoringDisable(false), [] { return true; });
+  EXPECT_EQ(false, command.get()->RunsWhenDisabled());
+}
+
+TEST_F(ConditionalCommandTest, OneTrueOneFalse) {
+  auto command =
+      cmd::Either(cmd::Idle().IgnoringDisable(true),
+                  cmd::Idle().IgnoringDisable(false), [] { return true; });
+  EXPECT_EQ(false, command.get()->RunsWhenDisabled());
+}
+
+TEST_F(ConditionalCommandTest, TwoFalseOneTrue) {
+  auto command =
+      cmd::Either(cmd::Idle().IgnoringDisable(false),
+                  cmd::Idle().IgnoringDisable(true), [] { return true; });
+  EXPECT_EQ(false, command.get()->RunsWhenDisabled());
+}
+
+TEST_F(ConditionalCommandTest, AllCancelSelf) {
+  auto command = cmd::Either(cmd::Idle().WithInterruptBehavior(
+                                 Command::InterruptionBehavior::kCancelSelf),
+                             cmd::Idle().WithInterruptBehavior(
+                                 Command::InterruptionBehavior::kCancelSelf),
+                             [] { return true; });
+  EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf,
+            command.get()->GetInterruptionBehavior());
+}
+
+TEST_F(ConditionalCommandTest, AllCancelIncoming) {
+  auto command =
+      cmd::Either(cmd::Idle().WithInterruptBehavior(
+                      Command::InterruptionBehavior::kCancelIncoming),
+                  cmd::Idle().WithInterruptBehavior(
+                      Command::InterruptionBehavior::kCancelIncoming),
+                  [] { return false; });
+  EXPECT_EQ(Command::InterruptionBehavior::kCancelIncoming,
+            command.get()->GetInterruptionBehavior());
+}
+
+TEST_F(ConditionalCommandTest, OneCancelSelfOneIncoming) {
+  auto command =
+      cmd::Either(cmd::Idle().WithInterruptBehavior(
+                      Command::InterruptionBehavior::kCancelSelf),
+                  cmd::Idle().WithInterruptBehavior(
+                      Command::InterruptionBehavior::kCancelIncoming),
+                  [] { return false; });
+  EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf,
+            command.get()->GetInterruptionBehavior());
+}
+
+TEST_F(ConditionalCommandTest, OneCancelIncomingOneSelf) {
+  auto command =
+      cmd::Either(cmd::Idle().WithInterruptBehavior(
+                      Command::InterruptionBehavior::kCancelIncoming),
+                  cmd::Idle().WithInterruptBehavior(
+                      Command::InterruptionBehavior::kCancelSelf),
+                  [] { return false; });
+  EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf,
+            command.get()->GetInterruptionBehavior());
+}

commandsv2/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <utility>
+
+#include "CommandTestBase.h"
+#include "frc2/command/Commands.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class DefaultCommandTest : public CommandTestBase {};
+
+TEST_F(DefaultCommandTest, DefaultCommandSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem subsystem;
+
+  auto command = cmd::Idle({&subsystem});
+
+  scheduler.SetDefaultCommand(&subsystem, std::move(command));
+  auto handle = scheduler.GetDefaultCommand(&subsystem);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(handle));
+}
+
+TEST_F(DefaultCommandTest, DefaultCommandInterruptResume) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem subsystem;
+
+  auto command1 = cmd::Idle({&subsystem});
+  auto command2 = cmd::Idle({&subsystem});
+
+  scheduler.SetDefaultCommand(&subsystem, std::move(command1));
+  auto handle = scheduler.GetDefaultCommand(&subsystem);
+  scheduler.Run();
+  scheduler.Schedule(command2);
+
+  EXPECT_TRUE(scheduler.IsScheduled(command2));
+  EXPECT_FALSE(scheduler.IsScheduled(handle));
+
+  scheduler.Cancel(command2);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(handle));
+}

commandsv2/src/test/native/cpp/frc2/command/DeferredCommandTest.cpp

@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "CommandTestBase.h"
+#include "frc2/command/Commands.h"
+#include "frc2/command/DeferredCommand.h"
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+
+class DeferredFunctionsTest : public CommandTestBaseWithParam<bool> {};
+
+TEST_P(DeferredFunctionsTest, DeferredFunctions) {
+  int initializeCount = 0;
+  int executeCount = 0;
+  int isFinishedCount = 0;
+  int endCount = 0;
+  bool finished = false;
+
+  DeferredCommand deferred{[&] {
+                             return FunctionalCommand{
+                                 [&] { initializeCount++; },
+                                 [&] { executeCount++; },
+                                 [&](bool interrupted) {
+                                   EXPECT_EQ(interrupted, GetParam());
+                                   endCount++;
+                                 },
+                                 [&] {
+                                   isFinishedCount++;
+                                   return finished;
+                                 }}
+                                 .ToPtr();
+                           },
+                           {}};
+
+  deferred.Initialize();
+  EXPECT_EQ(1, initializeCount);
+  deferred.Execute();
+  EXPECT_EQ(1, executeCount);
+  EXPECT_FALSE(deferred.IsFinished());
+  EXPECT_EQ(1, isFinishedCount);
+  finished = true;
+  EXPECT_TRUE(deferred.IsFinished());
+  EXPECT_EQ(2, isFinishedCount);
+  deferred.End(GetParam());
+  EXPECT_EQ(1, endCount);
+}
+
+INSTANTIATE_TEST_SUITE_P(DeferredCommandTests, DeferredFunctionsTest,
+                         testing::Values(true, false));
+
+TEST(DeferredCommandTest, DeferredSupplierOnlyCalledDuringInit) {
+  int count = 0;
+  DeferredCommand command{[&count] {
+                            count++;
+                            return cmd::None();
+                          },
+                          {}};
+
+  EXPECT_EQ(0, count);
+  command.Initialize();
+  EXPECT_EQ(1, count);
+  command.Execute();
+  command.IsFinished();
+  command.End(false);
+  EXPECT_EQ(1, count);
+}
+
+TEST(DeferredCommandTest, DeferredRequirements) {
+  TestSubsystem subsystem;
+  DeferredCommand command{cmd::None, {&subsystem}};
+
+  EXPECT_TRUE(command.GetRequirements().contains(&subsystem));
+}

commandsv2/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp

@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "CommandTestBase.h"
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+class FunctionalCommandTest : public CommandTestBase {};
+
+TEST_F(FunctionalCommandTest, FunctionalCommandSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+  bool finished = false;
+
+  FunctionalCommand command(
+      [&counter] { counter++; }, [&counter] { counter++; },
+      [&counter](bool) { counter++; }, [&finished] { return finished; });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  EXPECT_EQ(4, counter);
+}

commandsv2/src/test/native/cpp/frc2/command/InstantCommandTest.cpp

@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "CommandTestBase.h"
+#include "frc2/command/Commands.h"
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+class InstantCommandTest : public CommandTestBase {};
+
+TEST_F(InstantCommandTest, InstantCommandSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  auto command = cmd::RunOnce([&counter] { counter++; });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(command));
+  EXPECT_EQ(counter, 1);
+}

commandsv2/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/simulation/SimHooks.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/NotifierCommand.h"
+
+using namespace frc2;
+using namespace std::chrono_literals;
+
+class NotifierCommandTest : public CommandTestBase {};
+
+TEST_F(NotifierCommandTest, NotifierCommandSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  frc::sim::PauseTiming();
+
+  int counter = 0;
+  NotifierCommand command([&] { counter++; }, 10_ms, {});
+
+  scheduler.Schedule(&command);
+  for (int i = 0; i < 5; ++i) {
+    frc::sim::StepTiming(5_ms);
+  }
+  scheduler.Cancel(&command);
+
+  frc::sim::ResumeTiming();
+
+  EXPECT_EQ(counter, 2);
+}

commandsv2/src/test/native/cpp/frc2/command/POVButtonTest.cpp

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/DriverStation.h>
+#include <frc/Joystick.h>
+#include <frc/simulation/JoystickSim.h>
+#include <gtest/gtest.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/POVButton.h"
+
+using namespace frc2;
+class POVButtonTest : public CommandTestBase {};
+
+TEST_F(POVButtonTest, SetPOV) {
+  frc::sim::JoystickSim joysim(1);
+  joysim.SetPOV(frc::DriverStation::kUp);
+  joysim.NotifyNewData();
+
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  frc::Joystick joy(1);
+  POVButton(&joy, frc::DriverStation::kRight).OnTrue(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  joysim.SetPOV(frc::DriverStation::kRight);
+  joysim.NotifyNewData();
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

commandsv2/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp

@@ -0,0 +1,124 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <memory>
+#include <utility>
+
+#include "CommandTestBase.h"
+#include "CompositionTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelCommandGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+
+  ParallelCommandGroup group(make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(2);
+  EXPECT_CALL(*command2, End(false));
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+
+  ParallelCommandGroup group(make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(2);
+  EXPECT_CALL(*command2, End(false)).Times(0);
+  EXPECT_CALL(*command2, End(true));
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  scheduler.Run();
+  scheduler.Cancel(&group);
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupNotScheduledCancel) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto group = cmd::Parallel(cmd::None(), cmd::None());
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupCopy) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command = cmd::WaitUntil([&finished] { return finished; });
+
+  auto group = cmd::Parallel(std::move(command));
+  scheduler.Schedule(group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupRequirement) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  auto command1 = cmd::RunOnce([] {}, {&requirement1, &requirement2});
+  auto command2 = cmd::RunOnce([] {}, {&requirement3});
+  auto command3 = cmd::RunOnce([] {}, {&requirement3, &requirement4});
+
+  auto group = cmd::Parallel(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(group);
+  scheduler.Schedule(command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(command3));
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+INSTANTIATE_MULTI_COMMAND_COMPOSITION_TEST_SUITE(ParallelCommandGroupTest,
+                                                 ParallelCommandGroup);

commandsv2/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp

@@ -0,0 +1,158 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "CommandTestBase.h"
+#include "CompositionTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelDeadlineGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelDeadlineGroup group(
+      std::move(command1Holder),
+      make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+                                            std::move(command3Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(2);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(false));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(2);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  command2->SetFinished(true);
+  scheduler.Run();
+  command1->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem subsystem;
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelDeadlineGroup group(
+      std::move(command1Holder),
+      make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
+                                            std::move(command3Holder)));
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(true));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(true));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(1);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  scheduler.Cancel(&group);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupNotScheduledCancel) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto group = cmd::Deadline(cmd::None(), cmd::None());
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopy) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command = cmd::WaitUntil([&finished] { return finished; });
+
+  auto group = cmd::Deadline(std::move(command));
+  scheduler.Schedule(group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirement) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  auto command1 = cmd::RunOnce([] {}, {&requirement1, &requirement2});
+  auto command2 = cmd::RunOnce([] {}, {&requirement3});
+  auto command3 = cmd::RunOnce([] {}, {&requirement3, &requirement4});
+
+  auto group = cmd::Deadline(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(group);
+  scheduler.Schedule(command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(command3));
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+class TestableDeadlineCommand : public ParallelDeadlineGroup {
+  static ParallelDeadlineGroup ToCommand(
+      std::vector<std::unique_ptr<Command>>&& commands) {
+    std::vector<std::unique_ptr<Command>> vec;
+    std::unique_ptr<Command> deadline = std::move(commands[0]);
+    for (unsigned int i = 1; i < commands.size(); i++) {
+      vec.emplace_back(std::move(commands[i]));
+    }
+    return ParallelDeadlineGroup(std::move(deadline), std::move(vec));
+  }
+
+ public:
+  explicit TestableDeadlineCommand(
+      std::vector<std::unique_ptr<Command>>&& commands)
+      : ParallelDeadlineGroup(ToCommand(std::move(commands))) {}
+};
+
+INSTANTIATE_MULTI_COMMAND_COMPOSITION_TEST_SUITE(ParallelDeadlineGroupTest,
+                                                 TestableDeadlineCommand);

commandsv2/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp

@@ -0,0 +1,211 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <memory>
+#include <utility>
+
+#include "CommandTestBase.h"
+#include "CompositionTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelRaceGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelRaceGroup group{make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(2);
+  EXPECT_CALL(*command1, End(true));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(2);
+  EXPECT_CALL(*command2, End(false));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(2);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelRaceGroup group{make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(true));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(true));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(1);
+  EXPECT_CALL(*command3, End(true));
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  scheduler.Cancel(&group);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceNotScheduledCancel) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto group = cmd::Race(cmd::None(), cmd::None());
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceCopy) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command = cmd::WaitUntil([&finished] { return finished; });
+
+  auto group = cmd::Race(std::move(command));
+  scheduler.Schedule(group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+TEST_F(ParallelRaceGroupTest, RaceGroupRequirement) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  auto command1 = cmd::RunOnce([] {}, {&requirement1, &requirement2});
+  auto command2 = cmd::RunOnce([] {}, {&requirement3});
+  auto command3 = cmd::RunOnce([] {}, {&requirement3, &requirement4});
+
+  auto group = cmd::Race(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(group);
+  scheduler.Schedule(command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(command3));
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnce) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished1 = false;
+  bool finished2 = false;
+  bool finished3 = false;
+
+  auto command1 = cmd::WaitUntil([&finished1] { return finished1; });
+  auto command2 = cmd::WaitUntil([&finished2] { return finished2; });
+  auto command3 = cmd::WaitUntil([&finished3] { return finished3; });
+
+  auto group1 = cmd::Sequence(std::move(command1), std::move(command2));
+  auto group2 = cmd::Race(std::move(group1), std::move(command3));
+
+  scheduler.Schedule(group2);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(group2));
+  finished1 = true;
+  scheduler.Run();
+  finished2 = true;
+  EXPECT_NO_FATAL_FAILURE(scheduler.Run());
+  EXPECT_FALSE(scheduler.IsScheduled(group2));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTwice) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  ParallelRaceGroup group{make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize()).Times(2);
+  EXPECT_CALL(*command1, Execute()).Times(5);
+  EXPECT_CALL(*command1, End(true)).Times(2);
+
+  EXPECT_CALL(*command2, Initialize()).Times(2);
+  EXPECT_CALL(*command2, Execute()).Times(5);
+  EXPECT_CALL(*command2, End(false)).Times(2);
+
+  EXPECT_CALL(*command3, Initialize()).Times(2);
+  EXPECT_CALL(*command3, Execute()).Times(5);
+  EXPECT_CALL(*command3, End(true)).Times(2);
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+
+  command2->SetFinished(false);
+
+  scheduler.Schedule(&group);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+
+  command2->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+INSTANTIATE_MULTI_COMMAND_COMPOSITION_TEST_SUITE(ParallelRaceGroupTest,
+                                                 ParallelRaceGroup);

commandsv2/src/test/native/cpp/frc2/command/PrintCommandTest.cpp

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc2/command/Commands.h>
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/PrintCommand.h"
+
+using namespace frc2;
+class PrintCommandTest : public CommandTestBase {};
+
+TEST_F(PrintCommandTest, PrintCommandSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = cmd::Print("Test!");
+
+  testing::internal::CaptureStdout();
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+
+  EXPECT_TRUE(std::regex_search(testing::internal::GetCapturedStdout(),
+                                std::regex("Test!")));
+
+  EXPECT_FALSE(scheduler.IsScheduled(command));
+}

commandsv2/src/test/native/cpp/frc2/command/ProxyCommandTest.cpp

@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc2/command/Commands.h>
+
+#include <memory>
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ProxyCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ProxyCommandTest : public CommandTestBase {};
+
+TEST_F(ProxyCommandTest, NonOwningCommandSchedule) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool scheduled = false;
+
+  InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
+
+  ProxyCommand command(&toSchedule);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduled);
+}
+
+TEST_F(ProxyCommandTest, NonOwningCommandEnd) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool finished = false;
+
+  WaitUntilCommand toSchedule([&finished] { return finished; });
+
+  ProxyCommand command(&toSchedule);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ProxyCommandTest, OwningCommandSchedule) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool scheduled = false;
+
+  auto command = cmd::RunOnce([&scheduled] { scheduled = true; }).AsProxy();
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduled);
+}
+
+TEST_F(ProxyCommandTest, OwningCommandEnd) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool finished = false;
+
+  auto command = cmd::WaitUntil([&finished] { return finished; }).AsProxy();
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(command));
+  finished = true;
+  scheduler.Run();
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(command));
+}

commandsv2/src/test/native/cpp/frc2/command/RepeatCommandTest.cpp

@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "CommandTestBase.h"
+#include "CompositionTestBase.h"
+#include "frc2/command/FunctionalCommand.h"
+#include "frc2/command/RepeatCommand.h"
+
+using namespace frc2;
+class RepeatCommandTest : public CommandTestBase {};
+
+TEST_F(RepeatCommandTest, CallsMethodsCorrectly) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int initCounter = 0;
+  int exeCounter = 0;
+  int isFinishedCounter = 0;
+  int endCounter = 0;
+  bool isFinishedHook = false;
+
+  auto command =
+      FunctionalCommand([&initCounter] { initCounter++; },
+                        [&exeCounter] { exeCounter++; },
+                        [&endCounter](bool interrupted) { endCounter++; },
+                        [&isFinishedCounter, &isFinishedHook] {
+                          isFinishedCounter++;
+                          return isFinishedHook;
+                        })
+          .Repeatedly();
+
+  EXPECT_EQ(0, initCounter);
+  EXPECT_EQ(0, exeCounter);
+  EXPECT_EQ(0, isFinishedCounter);
+  EXPECT_EQ(0, endCounter);
+
+  scheduler.Schedule(command);
+  EXPECT_EQ(1, initCounter);
+  EXPECT_EQ(0, exeCounter);
+  EXPECT_EQ(0, isFinishedCounter);
+  EXPECT_EQ(0, endCounter);
+
+  isFinishedHook = false;
+  scheduler.Run();
+  EXPECT_EQ(1, initCounter);
+  EXPECT_EQ(1, exeCounter);
+  EXPECT_EQ(1, isFinishedCounter);
+  EXPECT_EQ(0, endCounter);
+
+  isFinishedHook = true;
+  scheduler.Run();
+  EXPECT_EQ(1, initCounter);
+  EXPECT_EQ(2, exeCounter);
+  EXPECT_EQ(2, isFinishedCounter);
+  EXPECT_EQ(1, endCounter);
+
+  isFinishedHook = false;
+  scheduler.Run();
+  EXPECT_EQ(2, initCounter);
+  EXPECT_EQ(3, exeCounter);
+  EXPECT_EQ(3, isFinishedCounter);
+  EXPECT_EQ(1, endCounter);
+
+  isFinishedHook = true;
+  scheduler.Run();
+  EXPECT_EQ(2, initCounter);
+  EXPECT_EQ(4, exeCounter);
+  EXPECT_EQ(4, isFinishedCounter);
+  EXPECT_EQ(2, endCounter);
+
+  command.Cancel();
+  EXPECT_EQ(2, initCounter);
+  EXPECT_EQ(4, exeCounter);
+  EXPECT_EQ(4, isFinishedCounter);
+  EXPECT_EQ(2, endCounter);
+}
+
+INSTANTIATE_SINGLE_COMMAND_COMPOSITION_TEST_SUITE(RepeatCommandTest,
+                                                  RepeatCommand);

commandsv2/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp

@@ -0,0 +1,153 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/ParallelCommandGroup.h"
+#include "frc2/command/ParallelDeadlineGroup.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/SelectCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class RobotDisabledCommandTest : public CommandTestBase {};
+
+TEST_F(RobotDisabledCommandTest, RobotDisabledCommandCancel) {
+  CommandScheduler scheduler = GetScheduler();
+
+  MockCommand command({}, false, false);
+
+  EXPECT_CALL(command, End(true));
+
+  SetDSEnabled(true);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+  SetDSEnabled(false);
+
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(RobotDisabledCommandTest, RunWhenDisabled) {
+  CommandScheduler scheduler = GetScheduler();
+
+  MockCommand command1;
+  MockCommand command2;
+
+  scheduler.Schedule(&command1);
+
+  SetDSEnabled(false);
+
+  scheduler.Run();
+
+  scheduler.Schedule(&command2);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(RobotDisabledCommandTest, SequentialGroupRunWhenDisabled) {
+  CommandScheduler scheduler = GetScheduler();
+
+  SequentialCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
+  SequentialCommandGroup dontRunWhenDisabled{MockCommand(),
+                                             MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelGroupRunWhenDisabled) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
+  ParallelCommandGroup dontRunWhenDisabled{MockCommand(),
+                                           MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelRaceRunWhenDisabled) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelRaceGroup runWhenDisabled{MockCommand(), MockCommand()};
+  ParallelRaceGroup dontRunWhenDisabled{MockCommand(),
+                                        MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ParallelDeadlineRunWhenDisabled) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelDeadlineGroup runWhenDisabled{MockCommand(), MockCommand()};
+  ParallelDeadlineGroup dontRunWhenDisabled{MockCommand(),
+                                            MockCommand({}, false, false)};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, ConditionalCommandRunWhenDisabled) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ConditionalCommand runWhenDisabled{MockCommand(), MockCommand(),
+                                     [] { return true; }};
+  ConditionalCommand dontRunWhenDisabled{
+      MockCommand(), MockCommand({}, false, false), [] { return true; }};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}
+
+TEST_F(RobotDisabledCommandTest, SelectCommandRunWhenDisabled) {
+  CommandScheduler scheduler = GetScheduler();
+
+  SelectCommand<int> runWhenDisabled{[] { return 1; },
+                                     std::pair(1, MockCommand()),
+                                     std::pair(1, MockCommand())};
+  SelectCommand<int> dontRunWhenDisabled{
+      [] { return 1; }, std::pair(1, MockCommand()),
+      std::pair(1, MockCommand({}, false, false))};
+
+  SetDSEnabled(false);
+
+  scheduler.Schedule(&runWhenDisabled);
+  scheduler.Schedule(&dontRunWhenDisabled);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
+  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
+}

commandsv2/src/test/native/cpp/frc2/command/RunCommandTest.cpp

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc2/command/Commands.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class RunCommandTest : public CommandTestBase {};
+
+TEST_F(RunCommandTest, RunCommandSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  auto command = cmd::Run([&counter] { counter++; });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_EQ(3, counter);
+}

commandsv2/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ScheduleCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class ScheduleCommandTest : public CommandTestBase {};
+
+TEST_F(ScheduleCommandTest, ScheduleCommandSchedule) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool scheduled = false;
+
+  InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
+
+  ScheduleCommand command(&toSchedule);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduled);
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

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

@@ -0,0 +1,226 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc2/command/Commands.h>
+
+#include <utility>
+
+#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 {};
+
+TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = cmd::None();
+
+  int counter = 0;
+
+  scheduler.OnCommandInitialize([&counter](const Command&) { counter++; });
+  scheduler.OnCommandExecute([&counter](const Command&) { counter++; });
+  scheduler.OnCommandFinish([&counter](const Command&) { counter++; });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+
+  EXPECT_EQ(counter, 3);
+}
+
+TEST_F(SchedulerTest, SchedulerLambdaInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = cmd::Idle();
+
+  int counter = 0;
+
+  scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+  scheduler.Cancel(command);
+
+  EXPECT_EQ(counter, 1);
+}
+
+TEST_F(SchedulerTest, SchedulerLambdaInterruptNoCause) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  scheduler.OnCommandInterrupt(
+      [&counter](const Command&, const std::optional<Command*>& interruptor) {
+        EXPECT_FALSE(interruptor);
+        counter++;
+      });
+
+  auto command = cmd::Idle();
+
+  scheduler.Schedule(command);
+  scheduler.Cancel(command);
+
+  EXPECT_EQ(1, counter);
+}
+
+TEST_F(SchedulerTest, SchedulerLambdaInterruptCause) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  TestSubsystem subsystem{};
+  auto command = cmd::Idle({&subsystem});
+  InstantCommand interruptor([] {}, {&subsystem});
+
+  scheduler.OnCommandInterrupt(
+      [&](const Command&, const std::optional<Command*>& cause) {
+        ASSERT_TRUE(cause);
+        EXPECT_EQ(&interruptor, *cause);
+        counter++;
+      });
+
+  scheduler.Schedule(command);
+  scheduler.Schedule(&interruptor);
+
+  EXPECT_EQ(1, counter);
+}
+
+TEST_F(SchedulerTest, SchedulerLambdaInterruptCauseInRunLoop) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  TestSubsystem subsystem{};
+  auto command = cmd::Idle({&subsystem});
+  InstantCommand interruptor([] {}, {&subsystem});
+  // This command will schedule interruptor in execute() inside the run loop
+  auto interruptorScheduler =
+      cmd::RunOnce([&] { scheduler.Schedule(&interruptor); });
+
+  scheduler.OnCommandInterrupt(
+      [&](const Command&, const std::optional<Command*>& cause) {
+        ASSERT_TRUE(cause);
+        EXPECT_EQ(&interruptor, *cause);
+        counter++;
+      });
+
+  scheduler.Schedule(command);
+  scheduler.Schedule(interruptorScheduler);
+
+  scheduler.Run();
+
+  EXPECT_EQ(1, counter);
+}
+
+TEST_F(SchedulerTest, RegisterSubsystem) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+  TestSubsystem system{[&counter] { counter++; }};
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.RegisterSubsystem(&system));
+
+  scheduler.Run();
+  EXPECT_EQ(counter, 1);
+}
+
+TEST_F(SchedulerTest, UnregisterSubsystem) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+  TestSubsystem system{[&counter] { counter++; }};
+
+  scheduler.RegisterSubsystem(&system);
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system));
+
+  scheduler.Run();
+  ASSERT_EQ(counter, 0);
+}
+
+TEST_F(SchedulerTest, SchedulerCancelAll) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command1 = cmd::Idle();
+  auto command2 = cmd::Idle();
+
+  int counter = 0;
+
+  scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; });
+  scheduler.OnCommandInterrupt(
+      [](const Command&, const std::optional<Command*>& interruptor) {
+        EXPECT_FALSE(interruptor);
+      });
+
+  scheduler.Schedule(command1);
+  scheduler.Schedule(command2);
+  scheduler.Run();
+  scheduler.CancelAll();
+
+  EXPECT_EQ(counter, 2);
+}
+
+TEST_F(SchedulerTest, ScheduleScheduledNoOp) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  auto command = cmd::StartEnd([&counter] { counter++; }, [] {});
+
+  scheduler.Schedule(command);
+  scheduler.Schedule(command);
+
+  EXPECT_EQ(counter, 1);
+}
+
+class TrackDestroyCommand
+    : public frc2::CommandHelper<Command, TrackDestroyCommand> {
+ public:
+  explicit TrackDestroyCommand(wpi::unique_function<void()> deleteFunc)
+      : m_deleteFunc{std::move(deleteFunc)} {}
+  TrackDestroyCommand(TrackDestroyCommand&& other)
+      : m_deleteFunc{std::exchange(other.m_deleteFunc, [] {})} {}
+  TrackDestroyCommand& operator=(TrackDestroyCommand&& other) {
+    m_deleteFunc = std::exchange(other.m_deleteFunc, [] {});
+    return *this;
+  }
+  ~TrackDestroyCommand() override { m_deleteFunc(); }
+
+ private:
+  wpi::unique_function<void()> m_deleteFunc;
+};
+
+TEST_F(SchedulerTest, ScheduleCommandPtr) {
+  CommandScheduler scheduler = GetScheduler();
+  int destructionCounter = 0;
+  int runCounter = 0;
+
+  bool finish = false;
+
+  {
+    auto commandPtr =
+        TrackDestroyCommand([&destructionCounter] { destructionCounter++; })
+            .AlongWith(
+                frc2::InstantCommand([&runCounter] { runCounter++; }).ToPtr())
+            .Until([&finish] { return finish; });
+    EXPECT_EQ(destructionCounter, 0) << "Composition should not delete command";
+
+    scheduler.Schedule(std::move(commandPtr));
+    EXPECT_EQ(destructionCounter, 0)
+        << "Scheduling should not delete CommandPtr";
+  }
+  EXPECT_EQ(destructionCounter, 0)
+      << "Scheduler should own CommandPtr after scheduling";
+  scheduler.Run();
+  EXPECT_EQ(runCounter, 1);
+  EXPECT_EQ(destructionCounter, 0) << "Scheduler should not destroy CommandPtr "
+                                      "until command lifetime is complete";
+  finish = true;
+  scheduler.Run();
+  EXPECT_EQ(runCounter, 1);
+  EXPECT_EQ(destructionCounter, 1)
+      << "Scheduler should delete command after command completes";
+}

commandsv2/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp

@@ -0,0 +1,386 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc2/command/Commands.h>
+
+#include <utility>
+
+#include <gtest/gtest.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/Command.h"
+#include "frc2/command/CommandHelper.h"
+#include "frc2/command/FunctionalCommand.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+
+class SchedulingRecursionTest
+    : public CommandTestBaseWithParam<Command::InterruptionBehavior> {};
+
+class SelfCancellingCommand
+    : public CommandHelper<Command, SelfCancellingCommand> {
+ public:
+  SelfCancellingCommand(CommandScheduler* scheduler, int& counter,
+                        Subsystem* requirement,
+                        Command::InterruptionBehavior interruptionBehavior =
+                            Command::InterruptionBehavior::kCancelSelf)
+      : m_scheduler(scheduler),
+        m_counter(counter),
+        m_interrupt(interruptionBehavior) {
+    AddRequirements(requirement);
+  }
+
+  void Initialize() override { m_scheduler->Cancel(this); }
+
+  void End(bool interrupted) override { m_counter++; }
+
+  InterruptionBehavior GetInterruptionBehavior() const override {
+    return m_interrupt;
+  }
+
+ private:
+  CommandScheduler* m_scheduler;
+  int& m_counter;
+  InterruptionBehavior m_interrupt;
+};
+
+/**
+ * Checks <a
+ * href="https://github.com/wpilibsuite/allwpilib/issues/4259">wpilibsuite/allwpilib#4259</a>.
+ */
+TEST_P(SchedulingRecursionTest, CancelFromInitialize) {
+  CommandScheduler scheduler = GetScheduler();
+  bool hasOtherRun = false;
+  int counter = 0;
+  TestSubsystem requirement;
+  SelfCancellingCommand selfCancels{&scheduler, counter, &requirement,
+                                    GetParam()};
+  auto other = cmd::Run([&hasOtherRun] { hasOtherRun = true; }, {&requirement});
+
+  scheduler.Schedule(&selfCancels);
+  scheduler.Run();
+  scheduler.Schedule(other);
+
+  EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
+  EXPECT_TRUE(scheduler.IsScheduled(other));
+  EXPECT_EQ(1, counter);
+  scheduler.Run();
+  EXPECT_TRUE(hasOtherRun);
+}
+
+TEST_F(SchedulingRecursionTest, CancelFromInitializeAction) {
+  CommandScheduler scheduler = GetScheduler();
+  bool hasOtherRun = false;
+  int counter = 0;
+  TestSubsystem requirement;
+  FunctionalCommand selfCancels{[] {},
+                                [] {},
+                                [&counter](bool) { counter++; },
+                                [] { return false; },
+                                {&requirement}};
+  auto other = cmd::Run([&hasOtherRun] { hasOtherRun = true; }, {&requirement});
+  scheduler.OnCommandInitialize([&scheduler, &selfCancels](const Command&) {
+    scheduler.Cancel(&selfCancels);
+  });
+  scheduler.Schedule(&selfCancels);
+  scheduler.Run();
+  scheduler.Schedule(other);
+
+  EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
+  EXPECT_TRUE(scheduler.IsScheduled(other));
+  EXPECT_EQ(1, counter);
+  scheduler.Run();
+  EXPECT_TRUE(hasOtherRun);
+}
+
+TEST_P(SchedulingRecursionTest,
+       DefaultCommandGetsRescheduledAfterSelfCanceling) {
+  CommandScheduler scheduler = GetScheduler();
+  bool hasOtherRun = false;
+  int counter = 0;
+  TestSubsystem requirement;
+  SelfCancellingCommand selfCancels{&scheduler, counter, &requirement,
+                                    GetParam()};
+  auto other = cmd::Run([&hasOtherRun] { hasOtherRun = true; }, {&requirement});
+  scheduler.SetDefaultCommand(&requirement, std::move(other));
+
+  scheduler.Schedule(&selfCancels);
+  scheduler.Run();
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
+  EXPECT_TRUE(scheduler.IsScheduled(scheduler.GetDefaultCommand(&requirement)));
+  EXPECT_EQ(1, counter);
+  scheduler.Run();
+  EXPECT_TRUE(hasOtherRun);
+}
+
+class CancelEndCommand : public CommandHelper<Command, CancelEndCommand> {
+ public:
+  CancelEndCommand(CommandScheduler* scheduler, int& counter)
+      : m_scheduler(scheduler), m_counter(counter) {}
+
+  void End(bool interrupted) override {
+    m_counter++;
+    m_scheduler->Cancel(this);
+  }
+
+ private:
+  CommandScheduler* m_scheduler;
+  int& m_counter;
+};
+
+TEST_F(SchedulingRecursionTest, CancelFromEnd) {
+  CommandScheduler scheduler = GetScheduler();
+  int counter = 0;
+  CancelEndCommand selfCancels{&scheduler, counter};
+
+  scheduler.Schedule(&selfCancels);
+
+  EXPECT_NO_THROW({ scheduler.Cancel(&selfCancels); });
+  EXPECT_EQ(1, counter);
+  EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
+}
+
+TEST_F(SchedulingRecursionTest, CancelFromInterruptAction) {
+  CommandScheduler scheduler = GetScheduler();
+  int counter = 0;
+  FunctionalCommand selfCancels{[] {}, [] {}, [](bool) {},
+                                [] { return false; }};
+  scheduler.OnCommandInterrupt([&](const Command&) {
+    counter++;
+    scheduler.Cancel(&selfCancels);
+  });
+  scheduler.Schedule(&selfCancels);
+
+  EXPECT_NO_THROW({ scheduler.Cancel(&selfCancels); });
+  EXPECT_EQ(1, counter);
+  EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
+}
+
+class EndCommand : public CommandHelper<Command, EndCommand> {
+ public:
+  explicit EndCommand(std::function<void(bool)> end) : m_end(end) {}
+  void End(bool interrupted) override { m_end(interrupted); }
+  bool IsFinished() override { return true; }
+
+ private:
+  std::function<void(bool)> m_end;
+};
+
+TEST_F(SchedulingRecursionTest, CancelFromEndLoop) {
+  CommandScheduler scheduler = GetScheduler();
+  int counter = 0;
+  EndCommand dCancelsAll([&](bool) {
+    counter++;
+    scheduler.CancelAll();
+  });
+  EndCommand cCancelsD([&](bool) {
+    counter++;
+    scheduler.Cancel(&dCancelsAll);
+  });
+  EndCommand bCancelsC([&](bool) {
+    counter++;
+    scheduler.Cancel(&cCancelsD);
+  });
+  EndCommand aCancelsB([&](bool) {
+    counter++;
+    scheduler.Cancel(&bCancelsC);
+  });
+  scheduler.Schedule(&aCancelsB);
+  scheduler.Schedule(&bCancelsC);
+  scheduler.Schedule(&cCancelsD);
+  scheduler.Schedule(&dCancelsAll);
+
+  EXPECT_NO_THROW({ scheduler.Cancel(&aCancelsB); });
+  EXPECT_EQ(4, counter);
+  EXPECT_FALSE(scheduler.IsScheduled(&aCancelsB));
+  EXPECT_FALSE(scheduler.IsScheduled(&bCancelsC));
+  EXPECT_FALSE(scheduler.IsScheduled(&cCancelsD));
+  EXPECT_FALSE(scheduler.IsScheduled(&dCancelsAll));
+}
+
+TEST_F(SchedulingRecursionTest, CancelFromEndLoopWhileInRunLoop) {
+  CommandScheduler scheduler = GetScheduler();
+  int counter = 0;
+
+  EndCommand dCancelsAll([&](bool) {
+    counter++;
+    scheduler.CancelAll();
+  });
+  EndCommand cCancelsD([&](bool) {
+    counter++;
+    scheduler.Cancel(&dCancelsAll);
+  });
+  EndCommand bCancelsC([&](bool) {
+    counter++;
+    scheduler.Cancel(&cCancelsD);
+  });
+  EndCommand aCancelsB([&](bool) {
+    counter++;
+    scheduler.Cancel(&bCancelsC);
+  });
+  scheduler.Schedule(&aCancelsB);
+  scheduler.Schedule(&bCancelsC);
+  scheduler.Schedule(&cCancelsD);
+  scheduler.Schedule(&dCancelsAll);
+
+  EXPECT_NO_THROW({ scheduler.Run(); });
+  EXPECT_EQ(4, counter);
+  EXPECT_FALSE(scheduler.IsScheduled(&aCancelsB));
+  EXPECT_FALSE(scheduler.IsScheduled(&bCancelsC));
+  EXPECT_FALSE(scheduler.IsScheduled(&cCancelsD));
+  EXPECT_FALSE(scheduler.IsScheduled(&dCancelsAll));
+}
+
+class MultiCancelCommand : public CommandHelper<Command, MultiCancelCommand> {
+ public:
+  MultiCancelCommand(CommandScheduler* scheduler, int& counter,
+                     Command* command)
+      : m_scheduler(scheduler), m_counter(counter), m_command(command) {}
+
+  void End(bool interrupted) override {
+    m_counter++;
+    m_scheduler->Cancel(m_command);
+    m_scheduler->Cancel(this);
+  }
+
+ private:
+  CommandScheduler* m_scheduler;
+  int& m_counter;
+  Command* m_command;
+};
+
+TEST_F(SchedulingRecursionTest, MultiCancelFromEnd) {
+  CommandScheduler scheduler = GetScheduler();
+  int counter = 0;
+  EndCommand bIncrementsCounter([&counter](bool) { counter++; });
+  MultiCancelCommand aCancelsB{&scheduler, counter, &bIncrementsCounter};
+
+  scheduler.Schedule(&aCancelsB);
+  scheduler.Schedule(&bIncrementsCounter);
+
+  EXPECT_NO_THROW({ scheduler.Cancel(&aCancelsB); });
+  EXPECT_EQ(2, counter);
+  EXPECT_FALSE(scheduler.IsScheduled(&aCancelsB));
+  EXPECT_FALSE(scheduler.IsScheduled(&bIncrementsCounter));
+}
+
+TEST_P(SchedulingRecursionTest, ScheduleFromEndCancel) {
+  CommandScheduler scheduler = GetScheduler();
+  int counter = 0;
+  TestSubsystem requirement;
+  SelfCancellingCommand selfCancels{&scheduler, counter, &requirement,
+                                    GetParam()};
+
+  scheduler.Schedule(&selfCancels);
+  EXPECT_NO_THROW({ scheduler.Cancel(&selfCancels); });
+  EXPECT_EQ(1, counter);
+  EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
+}
+
+TEST_P(SchedulingRecursionTest, ScheduleFromEndInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+  int counter = 0;
+  TestSubsystem requirement;
+  SelfCancellingCommand selfCancels{&scheduler, counter, &requirement,
+                                    GetParam()};
+  auto other = cmd::Idle({&requirement});
+
+  scheduler.Schedule(&selfCancels);
+  EXPECT_NO_THROW({ scheduler.Schedule(other); });
+  EXPECT_EQ(1, counter);
+  EXPECT_FALSE(scheduler.IsScheduled(&selfCancels));
+  EXPECT_TRUE(scheduler.IsScheduled(other));
+}
+
+TEST_F(SchedulingRecursionTest, ScheduleFromEndInterruptAction) {
+  CommandScheduler scheduler = GetScheduler();
+  int counter = 0;
+  TestSubsystem requirement;
+  auto selfCancels = cmd::Idle({&requirement});
+  auto other = cmd::Idle({&requirement});
+  scheduler.OnCommandInterrupt([&](const Command&) {
+    counter++;
+    scheduler.Schedule(other);
+  });
+  scheduler.Schedule(selfCancels);
+  EXPECT_NO_THROW({ scheduler.Schedule(other); });
+  EXPECT_EQ(1, counter);
+  EXPECT_FALSE(scheduler.IsScheduled(selfCancels));
+  EXPECT_TRUE(scheduler.IsScheduled(other));
+}
+
+TEST_F(SchedulingRecursionTest, CancelDefaultCommandFromEnd) {
+  CommandScheduler scheduler = GetScheduler();
+  int counter = 0;
+  TestSubsystem requirement;
+  FunctionalCommand defaultCommand{[] {},
+                                   [] {},
+                                   [&counter](bool) { counter++; },
+                                   [] { return false; },
+                                   {&requirement}};
+  auto other = cmd::Idle({&requirement});
+  FunctionalCommand cancelDefaultCommand{[] {}, [] {},
+                                         [&](bool) {
+                                           counter++;
+                                           scheduler.Schedule(other);
+                                         },
+                                         [] { return false; }};
+
+  EXPECT_NO_THROW({
+    scheduler.Schedule(&cancelDefaultCommand);
+    scheduler.SetDefaultCommand(&requirement, std::move(defaultCommand));
+
+    scheduler.Run();
+    scheduler.Cancel(&cancelDefaultCommand);
+  });
+  EXPECT_EQ(2, counter);
+  EXPECT_FALSE(scheduler.IsScheduled(&defaultCommand));
+  EXPECT_TRUE(scheduler.IsScheduled(other));
+}
+
+TEST_F(SchedulingRecursionTest, CancelNextCommandFromCommand) {
+  CommandScheduler scheduler = GetScheduler();
+
+  frc2::RunCommand* command1Ptr = nullptr;
+  frc2::RunCommand* command2Ptr = nullptr;
+  int counter = 0;
+
+  auto command1 = frc2::RunCommand([&counter, &command2Ptr, &scheduler] {
+    scheduler.Cancel(command2Ptr);
+    counter++;
+  });
+  auto command2 = frc2::RunCommand([&counter, &command1Ptr, &scheduler] {
+    scheduler.Cancel(command1Ptr);
+    counter++;
+  });
+
+  command1Ptr = &command1;
+  command2Ptr = &command2;
+
+  scheduler.Schedule(&command1);
+  scheduler.Schedule(&command2);
+  scheduler.Run();
+
+  EXPECT_EQ(counter, 1) << "Second command was run when it shouldn't have been";
+
+  // only one of the commands should be canceled.
+  EXPECT_FALSE(scheduler.IsScheduled(&command1) &&
+               scheduler.IsScheduled(&command2))
+      << "Both commands are running when only one should be";
+  // one of the commands shouldn't be canceled because the other one is canceled
+  // first
+  EXPECT_TRUE(scheduler.IsScheduled(&command1) ||
+              scheduler.IsScheduled(&command2))
+      << "Both commands are canceled when only one should be";
+
+  scheduler.Run();
+  EXPECT_EQ(counter, 2);
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    SchedulingRecursionTests, SchedulingRecursionTest,
+    testing::Values(Command::InterruptionBehavior::kCancelSelf,
+                    Command::InterruptionBehavior::kCancelIncoming));

commandsv2/src/test/native/cpp/frc2/command/SelectCommandTest.cpp

@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "CommandTestBase.h"
+#include "CompositionTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SelectCommand.h"
+
+using namespace frc2;
+class SelectCommandTest : public CommandTestBase {};
+
+TEST_F(SelectCommandTest, SelectCommand) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
+  MockCommand* mockptr = mock.get();
+
+  EXPECT_CALL(*mock, Initialize());
+  EXPECT_CALL(*mock, Execute()).Times(2);
+  EXPECT_CALL(*mock, End(false));
+
+  std::vector<std::pair<int, std::unique_ptr<Command>>> temp;
+
+  temp.emplace_back(std::pair(1, std::move(mock)));
+  temp.emplace_back(std::pair(2, std::make_unique<InstantCommand>()));
+  temp.emplace_back(std::pair(3, std::make_unique<InstantCommand>()));
+
+  SelectCommand<int> select([] { return 1; }, std::move(temp));
+
+  scheduler.Schedule(&select);
+  scheduler.Run();
+  mockptr->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&select));
+}
+
+TEST_F(SelectCommandTest, SelectCommandRequirement) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  auto command1 = cmd::RunOnce([] {}, {&requirement1, &requirement2});
+  auto command2 = cmd::RunOnce([] {}, {&requirement3});
+  auto command3 = cmd::RunOnce([] {}, {&requirement3, &requirement4});
+
+  auto select =
+      cmd::Select<int>([] { return 1; }, std::pair(1, std::move(command1)),
+                       std::pair(2, std::move(command2)));
+
+  scheduler.Schedule(select);
+  scheduler.Schedule(command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(command3));
+  EXPECT_FALSE(scheduler.IsScheduled(select));
+}
+
+class TestableSelectCommand : public SelectCommand<int> {
+  static std::vector<std::pair<int, std::unique_ptr<Command>>> ZipVector(
+      std::vector<std::unique_ptr<Command>>&& commands) {
+    std::vector<std::pair<int, std::unique_ptr<Command>>> vec;
+    int index = 0;
+    for (auto&& command : commands) {
+      vec.emplace_back(std::pair{index, std::move(command)});
+      index++;
+    }
+    return vec;
+  }
+
+ public:
+  explicit TestableSelectCommand(
+      std::vector<std::unique_ptr<Command>>&& commands)
+      : SelectCommand([] { return 0; }, ZipVector(std::move(commands))) {}
+};
+
+INSTANTIATE_MULTI_COMMAND_COMPOSITION_TEST_SUITE(SelectCommandTest,
+                                                 TestableSelectCommand);

commandsv2/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp

@@ -0,0 +1,141 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <memory>
+#include <utility>
+
+#include "CommandTestBase.h"
+#include "CompositionTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class SequentialCommandGroupTest : public CommandTestBase {};
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  SequentialCommandGroup group{make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(1);
+  EXPECT_CALL(*command2, End(false));
+
+  EXPECT_CALL(*command3, Initialize());
+  EXPECT_CALL(*command3, Execute()).Times(1);
+  EXPECT_CALL(*command3, End(false));
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  command2->SetFinished(true);
+  scheduler.Run();
+  command3->SetFinished(true);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+
+  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
+  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
+
+  MockCommand* command1 = command1Holder.get();
+  MockCommand* command2 = command2Holder.get();
+  MockCommand* command3 = command3Holder.get();
+
+  SequentialCommandGroup group{make_vector<std::unique_ptr<Command>>(
+      std::move(command1Holder), std::move(command2Holder),
+      std::move(command3Holder))};
+
+  EXPECT_CALL(*command1, Initialize());
+  EXPECT_CALL(*command1, Execute()).Times(1);
+  EXPECT_CALL(*command1, End(false));
+
+  EXPECT_CALL(*command2, Initialize());
+  EXPECT_CALL(*command2, Execute()).Times(0);
+  EXPECT_CALL(*command2, End(false)).Times(0);
+  EXPECT_CALL(*command2, End(true));
+
+  EXPECT_CALL(*command3, Initialize()).Times(0);
+  EXPECT_CALL(*command3, Execute()).Times(0);
+  EXPECT_CALL(*command3, End(false)).Times(0);
+  EXPECT_CALL(*command3, End(true)).Times(0);
+
+  scheduler.Schedule(&group);
+
+  command1->SetFinished(true);
+  scheduler.Run();
+  scheduler.Cancel(&group);
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupNotScheduledCancel) {
+  CommandScheduler scheduler = GetScheduler();
+
+  SequentialCommandGroup group{InstantCommand(), InstantCommand()};
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupCopy) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command = cmd::WaitUntil([&finished] { return finished; });
+
+  auto group = cmd::Sequence(std::move(command));
+  scheduler.Schedule(group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupRequirement) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  auto command1 = cmd::RunOnce([] {}, {&requirement1, &requirement2});
+  auto command2 = cmd::RunOnce([] {}, {&requirement3});
+  auto command3 = cmd::RunOnce([] {}, {&requirement3, &requirement4});
+
+  auto group = cmd::Sequence(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(group);
+  scheduler.Schedule(command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(command3));
+  EXPECT_FALSE(scheduler.IsScheduled(group));
+}
+
+INSTANTIATE_MULTI_COMMAND_COMPOSITION_TEST_SUITE(SequentialCommandGroupTest,
+                                                 SequentialCommandGroup);

commandsv2/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc2/command/Commands.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/StartEndCommand.h"
+
+using namespace frc2;
+class StartEndCommandTest : public CommandTestBase {};
+
+TEST_F(StartEndCommandTest, StartEndCommandSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  auto command =
+      cmd::StartEnd([&counter] { counter++; }, [&counter] { counter++; });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+  scheduler.Run();
+  scheduler.Cancel(command);
+
+  EXPECT_EQ(2, counter);
+}

commandsv2/src/test/native/cpp/frc2/command/WaitCommandTest.cpp

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc2/command/Commands.h>
+
+#include <frc/simulation/SimHooks.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitCommandTest : public CommandTestBase {};
+
+TEST_F(WaitCommandTest, WaitCommandSchedule) {
+  frc::sim::PauseTiming();
+
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = cmd::Wait(100_ms);
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(command));
+  frc::sim::StepTiming(110_ms);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(command));
+
+  frc::sim::ResumeTiming();
+}

commandsv2/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc2/command/Commands.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitUntilCommandTest : public CommandTestBase {};
+
+TEST_F(WaitUntilCommandTest, WaitUntilCommandSchedule) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command = cmd::WaitUntil([&finished] { return finished; });
+
+  scheduler.Schedule(command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(command));
+}

commandsv2/src/test/native/cpp/frc2/command/button/NetworkButtonTest.cpp

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+#include <networktables/NetworkTableInstance.h>
+
+#include "../CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/NetworkButton.h"
+
+using namespace frc2;
+
+class NetworkButtonTest : public CommandTestBase {
+ public:
+  NetworkButtonTest() {
+    inst = nt::NetworkTableInstance::Create();
+    inst.StartLocal();
+  }
+
+  ~NetworkButtonTest() override { nt::NetworkTableInstance::Destroy(inst); }
+
+  nt::NetworkTableInstance inst;
+};
+
+TEST_F(NetworkButtonTest, SetNetworkButton) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  auto pub = inst.GetTable("TestTable")->GetBooleanTopic("Test").Publish();
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  NetworkButton(inst, "TestTable", "Test").OnTrue(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pub.Set(true);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

commandsv2/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp

@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/DriverStation.h>
+#include <frc/simulation/DriverStationSim.h>
+
+#include "../CommandTestBase.h"
+#include "frc2/command/button/RobotModeTriggers.h"
+#include "frc2/command/button/Trigger.h"
+
+using namespace frc2;
+using namespace frc::sim;
+class RobotModeTriggersTest : public CommandTestBase {};
+
+TEST(RobotModeTriggersTest, Autonomous) {
+  DriverStationSim::ResetData();
+  DriverStationSim::SetAutonomous(true);
+  DriverStationSim::SetTest(false);
+  DriverStationSim::SetEnabled(true);
+  DriverStationSim::NotifyNewData();
+  Trigger autonomous = RobotModeTriggers::Autonomous();
+  EXPECT_TRUE(autonomous.Get());
+}
+
+TEST(RobotModeTriggersTest, Teleop) {
+  DriverStationSim::ResetData();
+  DriverStationSim::SetAutonomous(false);
+  DriverStationSim::SetTest(false);
+  DriverStationSim::SetEnabled(true);
+  DriverStationSim::NotifyNewData();
+  Trigger teleop = RobotModeTriggers::Teleop();
+  EXPECT_TRUE(teleop.Get());
+}
+
+TEST(RobotModeTriggersTest, Disabled) {
+  DriverStationSim::ResetData();
+  DriverStationSim::SetAutonomous(false);
+  DriverStationSim::SetTest(false);
+  DriverStationSim::SetEnabled(false);
+  DriverStationSim::NotifyNewData();
+  Trigger disabled = RobotModeTriggers::Disabled();
+  EXPECT_TRUE(disabled.Get());
+}
+
+TEST(RobotModeTriggersTest, TestMode) {
+  DriverStationSim::ResetData();
+  DriverStationSim::SetAutonomous(false);
+  DriverStationSim::SetTest(true);
+  DriverStationSim::SetEnabled(true);
+  DriverStationSim::NotifyNewData();
+  Trigger test = RobotModeTriggers::Test();
+  EXPECT_TRUE(test.Get());
+}

commandsv2/src/test/native/cpp/frc2/command/button/TriggerTest.cpp

@@ -0,0 +1,249 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <utility>
+
+#include <frc/simulation/SimHooks.h>
+#include <gtest/gtest.h>
+
+#include "../CommandTestBase.h"
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/Commands.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/Trigger.h"
+
+using namespace frc2;
+class TriggerTest : public CommandTestBase {};
+
+TEST_F(TriggerTest, OnTrue) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  Trigger([&pressed] { return pressed; }).OnTrue(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(TriggerTest, OnFalse) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = true;
+  Trigger([&pressed] { return pressed; }).OnFalse(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(TriggerTest, OnChange) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = true;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  Trigger([&pressed] { return pressed; }).OnChange(&command);
+  scheduler.Run();
+  EXPECT_FALSE(command.IsScheduled());
+  pressed = false;
+  scheduler.Run();
+  EXPECT_TRUE(command.IsScheduled());
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(command.IsScheduled());
+  finished = false;
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(command.IsScheduled());
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(command.IsScheduled());
+}
+
+TEST_F(TriggerTest, WhileTrueRepeatedly) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  int inits = 0;
+  int counter = 0;
+  bool pressed = false;
+  CommandPtr command =
+      FunctionalCommand([&inits] { inits++; }, [] {}, [](bool interrupted) {},
+                        [&counter] { return ++counter % 2 == 0; })
+          .Repeatedly();
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileTrue(std::move(command));
+  scheduler.Run();
+  EXPECT_EQ(0, inits);
+  pressed = true;
+  scheduler.Run();
+  EXPECT_EQ(1, inits);
+  scheduler.Run();
+  EXPECT_EQ(1, inits);
+  scheduler.Run();
+  EXPECT_EQ(2, inits);
+  pressed = false;
+  scheduler.Run();
+  EXPECT_EQ(2, inits);
+}
+
+TEST_F(TriggerTest, WhileTrueLambdaRun) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  int counter = 0;
+  bool pressed = false;
+  CommandPtr command = cmd::Run([&counter] { counter++; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileTrue(std::move(command));
+  scheduler.Run();
+  EXPECT_EQ(0, counter);
+  pressed = true;
+  scheduler.Run();
+  scheduler.Run();
+  EXPECT_EQ(2, counter);
+  pressed = false;
+  scheduler.Run();
+  EXPECT_EQ(2, counter);
+}
+
+TEST_F(TriggerTest, WhenTrueOnce) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  int startCounter = 0;
+  int endCounter = 0;
+  bool pressed = false;
+
+  CommandPtr command = cmd::StartEnd([&startCounter] { startCounter++; },
+                                     [&endCounter] { endCounter++; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileTrue(std::move(command));
+  scheduler.Run();
+  EXPECT_EQ(0, startCounter);
+  EXPECT_EQ(0, endCounter);
+  pressed = true;
+  scheduler.Run();
+  scheduler.Run();
+  EXPECT_EQ(1, startCounter);
+  EXPECT_EQ(0, endCounter);
+  pressed = false;
+  scheduler.Run();
+  EXPECT_EQ(1, startCounter);
+  EXPECT_EQ(1, endCounter);
+}
+
+TEST_F(TriggerTest, ToggleOnTrue) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool pressed = false;
+  int startCounter = 0;
+  int endCounter = 0;
+  CommandPtr command = cmd::StartEnd([&startCounter] { startCounter++; },
+                                     [&endCounter] { endCounter++; });
+
+  Trigger([&pressed] { return pressed; }).ToggleOnTrue(std::move(command));
+  scheduler.Run();
+  EXPECT_EQ(0, startCounter);
+  EXPECT_EQ(0, endCounter);
+  pressed = true;
+  scheduler.Run();
+  scheduler.Run();
+  EXPECT_EQ(1, startCounter);
+  EXPECT_EQ(0, endCounter);
+  pressed = false;
+  scheduler.Run();
+  EXPECT_EQ(1, startCounter);
+  EXPECT_EQ(0, endCounter);
+  pressed = true;
+  scheduler.Run();
+  EXPECT_EQ(1, startCounter);
+  EXPECT_EQ(1, endCounter);
+}
+
+TEST_F(TriggerTest, And) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed1 = false;
+  bool pressed2 = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  (Trigger([&pressed1] { return pressed1; }) && ([&pressed2] {
+     return pressed2;
+   })).OnTrue(&command);
+  pressed1 = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed2 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(TriggerTest, Or) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed1 = false;
+  bool pressed2 = false;
+  WaitUntilCommand command1([&finished] { return finished; });
+  WaitUntilCommand command2([&finished] { return finished; });
+
+  (Trigger([&pressed1] { return pressed1; }) || ([&pressed2] {
+     return pressed2;
+   })).OnTrue(&command1);
+  pressed1 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+
+  pressed1 = false;
+
+  (Trigger([&pressed1] { return pressed1; }) || ([&pressed2] {
+     return pressed2;
+   })).OnTrue(&command2);
+  pressed2 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(TriggerTest, Negate) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = true;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  (!Trigger([&pressed] { return pressed; })).OnTrue(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(TriggerTest, Debounce) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool pressed = false;
+  RunCommand command([] {});
+
+  Trigger([&pressed] { return pressed; }).Debounce(100_ms).OnTrue(&command);
+  pressed = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  frc::sim::StepTiming(300_ms);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}

commandsv2/src/test/native/cpp/frc2/command/make_vector.h

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+namespace frc2 {
+
+template <typename T = void, typename... Args,
+          typename CommonType = std::conditional_t<
+              std::same_as<T, void>, std::common_type_t<Args...>, T>>
+  requires((std::is_constructible_v<T, Args> &&
+            std::is_convertible_v<Args, T>) &&
+           ...)
+std::vector<CommonType> make_vector(Args&&... args) {
+  if constexpr (std::is_trivially_copyable_v<CommonType>) {
+    return std::vector<CommonType>{std::forward<Args>(args)...};
+  } else {
+    std::vector<CommonType> vec;
+    vec.reserve(sizeof...(Args));
+
+    using arr_t = int[];
+    [[maybe_unused]]
+    arr_t arr{0, (vec.emplace_back(std::forward<Args>(args)), 0)...};
+
+    return vec;
+  }
+}
+
+}  // namespace frc2

commandsv2/src/test/native/cpp/frc2/command/sysid/SysIdRoutineTest.cpp

@@ -0,0 +1,185 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc2/command/Subsystem.h>
+#include <frc2/command/sysid/SysIdRoutine.h>
+
+#include <utility>
+#include <vector>
+
+#include <frc/Timer.h>
+#include <frc/simulation/SimHooks.h>
+#include <gtest/gtest.h>
+#include <units/math.h>
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+enum StateTest {
+  Invalid,
+  InRecordStateQf,
+  InRecordStateQr,
+  InRecordStateDf,
+  InRecordStateDr,
+  InDrive,
+  InLog,
+  DoneWithRecordState
+};
+
+class SysIdRoutineTest : public ::testing::Test {
+ protected:
+  std::vector<StateTest> currentStateList{};
+  std::vector<units::volt_t> sentVoltages{};
+  frc2::Subsystem m_subsystem{};
+  frc2::sysid::SysIdRoutine m_sysidRoutine{
+      frc2::sysid::Config{
+          std::nullopt, std::nullopt, std::nullopt,
+          [this](frc::sysid::State state) {
+            switch (state) {
+              case frc::sysid::State::kQuasistaticForward:
+                currentStateList.emplace_back(StateTest::InRecordStateQf);
+                break;
+              case frc::sysid::State::kQuasistaticReverse:
+                currentStateList.emplace_back(StateTest::InRecordStateQr);
+                break;
+              case frc::sysid::State::kDynamicForward:
+                currentStateList.emplace_back(StateTest::InRecordStateDf);
+                break;
+              case frc::sysid::State::kDynamicReverse:
+                currentStateList.emplace_back(StateTest::InRecordStateDr);
+                break;
+              case frc::sysid::State::kNone:
+                currentStateList.emplace_back(StateTest::DoneWithRecordState);
+                break;
+            }
+          }},
+      frc2::sysid::Mechanism{
+          [this](units::volt_t driveVoltage) {
+            sentVoltages.emplace_back(driveVoltage);
+            currentStateList.emplace_back(StateTest::InDrive);
+          },
+          [this](frc::sysid::SysIdRoutineLog* log) {
+            currentStateList.emplace_back(StateTest::InLog);
+            log->Motor("Mock Motor").position(0_m).velocity(0_mps).voltage(0_V);
+          },
+          &m_subsystem}};
+
+  frc2::CommandPtr m_quasistaticForward{
+      m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kForward)};
+  frc2::CommandPtr m_quasistaticReverse{
+      m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kReverse)};
+  frc2::CommandPtr m_dynamicForward{
+      m_sysidRoutine.Dynamic(frc2::sysid::Direction::kForward)};
+  frc2::CommandPtr m_dynamicReverse{
+      m_sysidRoutine.Dynamic(frc2::sysid::Direction::kReverse)};
+
+  frc2::sysid::SysIdRoutine m_emptySysidRoutine{
+      frc2::sysid::Config{std::nullopt, std::nullopt, std::nullopt, nullptr},
+      frc2::sysid::Mechanism{[](units::volt_t driveVoltage) {}, nullptr,
+                             &m_subsystem}};
+
+  frc2::CommandPtr m_emptyRoutineForward{
+      m_emptySysidRoutine.Quasistatic(frc2::sysid::Direction::kForward)};
+
+  void RunCommand(frc2::CommandPtr command) {
+    command.get()->Initialize();
+    command.get()->Execute();
+    frc::sim::StepTiming(1_s);
+    command.get()->Execute();
+    command.get()->End(true);
+  }
+
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    frc2::CommandPtr m_quasistaticForward{
+        m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kForward)};
+    frc2::CommandPtr m_quasistaticReverse{
+        m_sysidRoutine.Quasistatic(frc2::sysid::Direction::kReverse)};
+    frc2::CommandPtr m_dynamicForward{
+        m_sysidRoutine.Dynamic(frc2::sysid::Direction::kForward)};
+    frc2::CommandPtr m_dynamicReverse{
+        m_sysidRoutine.Dynamic(frc2::sysid::Direction::kReverse)};
+  }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+TEST_F(SysIdRoutineTest, RecordStateBookendsMotorLogging) {
+  RunCommand(std::move(m_quasistaticForward));
+  std::vector<StateTest> expectedOrder{
+      StateTest::InDrive, StateTest::InLog, StateTest::InRecordStateQf,
+      StateTest::InDrive, StateTest::DoneWithRecordState};
+  EXPECT_TRUE(expectedOrder == currentStateList);
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  expectedOrder = std::vector<StateTest>{
+      StateTest::InDrive, StateTest::InLog, StateTest::InRecordStateDf,
+      StateTest::InDrive, StateTest::DoneWithRecordState};
+  RunCommand(std::move(m_dynamicForward));
+  EXPECT_TRUE(expectedOrder == currentStateList);
+  currentStateList.clear();
+  sentVoltages.clear();
+}
+
+TEST_F(SysIdRoutineTest, DeclareCorrectState) {
+  RunCommand(std::move(m_quasistaticForward));
+  EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+                        StateTest::InRecordStateQf) != currentStateList.end());
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_quasistaticReverse));
+  EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+                        StateTest::InRecordStateQr) != currentStateList.end());
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_dynamicForward));
+  EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+                        StateTest::InRecordStateDf) != currentStateList.end());
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_dynamicReverse));
+  EXPECT_TRUE(std::find(currentStateList.begin(), currentStateList.end(),
+                        StateTest::InRecordStateDr) != currentStateList.end());
+  currentStateList.clear();
+  sentVoltages.clear();
+}
+
+TEST_F(SysIdRoutineTest, OutputCorrectVoltage) {
+  RunCommand(std::move(m_quasistaticForward));
+  std::vector<units::volt_t> expectedVoltages{1_V, 0_V};
+  EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+  EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_quasistaticReverse));
+  expectedVoltages = std::vector<units::volt_t>{-1_V, 0_V};
+  EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+  EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_dynamicForward));
+  expectedVoltages = std::vector<units::volt_t>{7_V, 0_V};
+  EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+  EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+  currentStateList.clear();
+  sentVoltages.clear();
+
+  RunCommand(std::move(m_dynamicReverse));
+  expectedVoltages = std::vector<units::volt_t>{-7_V, 0_V};
+  EXPECT_NEAR_UNITS(expectedVoltages[0], sentVoltages[0], 1e-6_V);
+  EXPECT_NEAR_UNITS(expectedVoltages[1], sentVoltages[1], 1e-6_V);
+  currentStateList.clear();
+  sentVoltages.clear();
+}
+
+TEST_F(SysIdRoutineTest, EmptyLogFunc) {
+  RunCommand(std::move(m_emptyRoutineForward));
+  SUCCEED();
+}

commandsv2/src/test/native/cpp/main.cpp

@@ -0,0 +1,13 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+#include <hal/HALBase.h>
+
+int main(int argc, char** argv) {
+  HAL_Initialize(500, 0);
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

commandsv2/src/test/resources/META-INF/services/org.junit.jupiter.api.extension.Extension

@@ -0,0 +1 @@
+edu.wpi.first.wpilibj2.MockHardwareExtension