SCRIPT Generic Renames

commandsv2/src/test/native/cpp/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/command/CommandTestBase.hpp

@@ -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/wpi/command/CompositionTestBase.hpp

@@ -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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/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/wpi/command/make_vector.hpp

@@ -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/wpi/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();
+}