Add new C++ Command framework (#1785)

This is the C++ version of #1682.

The old command framework is still available, but will be deprecated.

Due to name conflicts, the new framework is in the frc2 namespace.
Eventually (after the old command framework is removed in a future year)
it will be moved into the main frc namespace.

wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp

@@ -0,0 +1,195 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/Trigger.h"
+#include "gtest/gtest.h"
+
+using namespace frc2;
+class ButtonTest : public CommandTestBase {};
+
+TEST_F(ButtonTest, WhenPressedTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  Trigger([&pressed] { return pressed; }).WhenActive(&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(ButtonTest, WhenReleasedTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = true;
+  Trigger([&pressed] { return pressed; }).WhenInactive(&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(ButtonTest, WhileHeldTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveContinous(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  finished = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhenHeldTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  finished = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+  pressed = true;
+  scheduler.Run();
+  pressed = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, ToggleWhenPressedTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).ToggleWhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  pressed = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, AndTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed1 = false;
+  bool pressed2 = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  (Trigger([&pressed1] { return pressed1; }) &&
+   Trigger([&pressed2] { return pressed2; }))
+      .WhenActive(&command);
+  pressed1 = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed2 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, OrTest) {
+  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; }) ||
+   Trigger([&pressed2] { return pressed2; }))
+      .WhenActive(&command1);
+  pressed1 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+
+  pressed1 = false;
+
+  (Trigger([&pressed1] { return pressed1; }) ||
+   Trigger([&pressed2] { return pressed2; }))
+      .WhenActive(&command2);
+  pressed2 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(ButtonTest, NegateTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = true;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  (!Trigger([&pressed] { return pressed; })).WhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, RValueButtonTest) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  int counter = 0;
+  bool pressed = false;
+
+  RunCommand command([&counter] { counter++; }, {});
+
+  Trigger([&pressed] { return pressed; }).WhenActive(std::move(command));
+  scheduler.Run();
+  EXPECT_EQ(counter, 0);
+  pressed = true;
+  scheduler.Run();
+  EXPECT_EQ(counter, 1);
+}

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

@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/PerpetualCommand.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/SequentialCommandGroup.h"
+
+using namespace frc2;
+class CommandDecoratorTest : public CommandTestBase {};
+
+TEST_F(CommandDecoratorTest, WithTimeoutTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = RunCommand([] {}, {}).WithTimeout(.1);
+
+  scheduler.Schedule(&command);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(150));
+
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, InterruptOnTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command =
+      RunCommand([] {}, {}).InterruptOn([&finished] { return finished; });
+
+  scheduler.Schedule(&command);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+
+  finished = true;
+
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, BeforeStartingTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command = InstantCommand([] {}, {}).BeforeStarting(
+      [&finished] { finished = true; });
+
+  scheduler.Schedule(&command);
+
+  EXPECT_TRUE(finished);
+
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(CommandDecoratorTest, WhenFinishedTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  auto command =
+      InstantCommand([] {}, {}).WhenFinished([&finished] { finished = true; });
+
+  scheduler.Schedule(&command);
+
+  EXPECT_FALSE(finished);
+
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  EXPECT_TRUE(finished);
+}
+
+TEST_F(CommandDecoratorTest, PerpetuallyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  auto command = InstantCommand([] {}, {}).Perpetually();
+
+  scheduler.Schedule(&command);
+
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}

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

@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.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 CommandRequirementsTest : public CommandTestBase {};
+
+TEST_F(CommandRequirementsTest, RequirementInterruptTest) {
+  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, RequirementUninterruptibleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement;
+
+  MockCommand command1({&requirement});
+  MockCommand command2({&requirement});
+
+  EXPECT_CALL(command1, Initialize());
+  EXPECT_CALL(command1, Execute()).Times(2);
+  EXPECT_CALL(command1, End(true)).Times(0);
+  EXPECT_CALL(command1, End(false)).Times(0);
+
+  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(false, &command1);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  scheduler.Schedule(&command2);
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+  EXPECT_FALSE(scheduler.IsScheduled(&command2));
+  scheduler.Run();
+}
+
+TEST_F(CommandRequirementsTest, DefaultCommandRequirementErrorTest) {
+  TestSubsystem requirement1;
+  ErrorConfirmer confirmer("require");
+
+  MockCommand command1;
+
+  requirement1.SetDefaultCommand(std::move(command1));
+
+  EXPECT_TRUE(requirement1.GetDefaultCommand() == NULL);
+}

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

@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+
+using namespace frc2;
+class CommandScheduleTest : public CommandTestBase {};
+
+TEST_F(CommandScheduleTest, InstantScheduleTest) {
+  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, SingleIterationScheduleTest) {
+  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, MultiScheduleTest) {
+  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, SchedulerCancelTest) {
+  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, NotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+  MockCommand command;
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command));
+}

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

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+
+using namespace frc2;
+
+CommandTestBase::CommandTestBase() {
+  auto& scheduler = CommandScheduler::GetInstance();
+  scheduler.CancelAll();
+  scheduler.Enable();
+  scheduler.ClearButtons();
+}
+
+CommandScheduler CommandTestBase::GetScheduler() { return CommandScheduler(); }
+
+void CommandTestBase::SetUp() {
+  HALSIM_SetDriverStationEnabled(true);
+  while (!HALSIM_GetDriverStationEnabled()) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+}
+
+void CommandTestBase::TearDown() {
+  CommandScheduler::GetInstance().ClearButtons();
+}
+
+void CommandTestBase::SetDSEnabled(bool enabled) {
+  HALSIM_SetDriverStationEnabled(enabled);
+  while (HALSIM_GetDriverStationEnabled() != static_cast<int>(enabled)) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
+}

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

@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <regex>
+#include <utility>
+
+#include <mockdata/MockHooks.h>
+
+#include "ErrorConfirmer.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/SetUtilities.h"
+#include "frc2/command/SubsystemBase.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+#include "make_vector.h"
+#include "simulation/DriverStationSim.h"
+
+namespace frc2 {
+class CommandTestBase : public ::testing::Test {
+ public:
+  CommandTestBase();
+
+  class TestSubsystem : public SubsystemBase {};
+
+ protected:
+  class MockCommand : public Command {
+   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));
+    }
+
+    MockCommand(std::initializer_list<Subsystem*> 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) : Command{} {}
+
+    void SetFinished(bool finished) {
+      EXPECT_CALL(*this, IsFinished())
+          .WillRepeatedly(::testing::Return(finished));
+    }
+
+    ~MockCommand() {
+      auto& scheduler = CommandScheduler::GetInstance();
+      scheduler.Cancel(this);
+    }
+
+   protected:
+    std::unique_ptr<Command> TransferOwnership() && {
+      return std::make_unique<MockCommand>(std::move(*this));
+    }
+
+   private:
+    wpi::SmallSet<Subsystem*, 4> m_requirements;
+  };
+
+  CommandScheduler GetScheduler();
+
+  void SetUp() override;
+
+  void TearDown() override;
+
+  void SetDSEnabled(bool enabled);
+};
+}  // namespace frc2

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

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SelectCommand.h"
+
+using namespace frc2;
+class ConditionalCommandTest : public CommandTestBase {};
+
+TEST_F(ConditionalCommandTest, ConditionalCommandScheduleTest) {
+  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, ConditionalCommandRequirementTest) {
+  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));
+}

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

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class DefaultCommandTest : public CommandTestBase {};
+
+TEST_F(DefaultCommandTest, DefaultCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem subsystem;
+
+  RunCommand command1([] {}, {&subsystem});
+
+  scheduler.SetDefaultCommand(&subsystem, std::move(command1));
+  auto handle = scheduler.GetDefaultCommand(&subsystem);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(handle));
+}
+
+TEST_F(DefaultCommandTest, DefaultCommandInterruptResumeTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem subsystem;
+
+  RunCommand command1([] {}, {&subsystem});
+  RunCommand command2([] {}, {&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));
+}

wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp

@@ -0,0 +1,20 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "ErrorConfirmer.h"
+
+ErrorConfirmer* ErrorConfirmer::instance;
+
+int32_t ErrorConfirmer::HandleError(HAL_Bool isError, int32_t errorCode,
+                                    HAL_Bool isLVCode, const char* details,
+                                    const char* location, const char* callStack,
+                                    HAL_Bool printMsg) {
+  if (std::regex_search(details, std::regex(instance->m_msg))) {
+    instance->ConfirmError();
+  }
+  return 1;
+}

wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h

@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <regex>
+
+#include <mockdata/MockHooks.h>
+
+#include "gmock/gmock.h"
+#include "simulation/DriverStationSim.h"
+
+class ErrorConfirmer {
+ public:
+  explicit ErrorConfirmer(const char* msg) : m_msg(msg) {
+    if (instance != nullptr) return;
+    HALSIM_SetSendError(HandleError);
+    EXPECT_CALL(*this, ConfirmError());
+    instance = this;
+  }
+
+  ~ErrorConfirmer() {
+    HALSIM_SetSendError(nullptr);
+    instance = nullptr;
+  }
+
+  MOCK_METHOD0(ConfirmError, void());
+
+  const char* m_msg;
+
+  static int32_t HandleError(HAL_Bool isError, int32_t errorCode,
+                             HAL_Bool isLVCode, const char* details,
+                             const char* location, const char* callStack,
+                             HAL_Bool printMsg);
+
+ private:
+  static ErrorConfirmer* instance;
+};

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

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/FunctionalCommand.h"
+
+using namespace frc2;
+class FunctionalCommandTest : public CommandTestBase {};
+
+TEST_F(FunctionalCommandTest, FunctionalCommandScheduleTest) {
+  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);
+}

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

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+
+using namespace frc2;
+class InstantCommandTest : public CommandTestBase {};
+
+TEST_F(InstantCommandTest, InstantCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  InstantCommand command([&counter] { counter++; }, {});
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  EXPECT_EQ(counter, 1);
+}

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

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/NotifierCommand.h"
+
+using namespace frc2;
+class NotifierCommandTest : public CommandTestBase {};
+
+#ifdef __APPLE__
+TEST_F(NotifierCommandTest, DISABLED_NotifierCommandScheduleTest) {
+#else
+TEST_F(NotifierCommandTest, NotifierCommandScheduleTest) {
+#endif
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  NotifierCommand command([&counter] { counter++; }, 0.01, {});
+
+  scheduler.Schedule(&command);
+  std::this_thread::sleep_for(std::chrono::milliseconds(250));
+  scheduler.Cancel(&command);
+
+  EXPECT_NEAR(.01 * counter, .25, .025);
+}

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

@@ -0,0 +1,120 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.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, ParallelGroupScheduleTest) {
+  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(tcb::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, ParallelGroupInterruptTest) {
+  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(tcb::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, ParallelGroupNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelCommandGroup group((InstantCommand(), InstantCommand()));
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  ParallelCommandGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelCommandGroupTest, ParallelGroupRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ParallelCommandGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}

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

@@ -0,0 +1,136 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.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, DeadlineGroupScheduleTest) {
+  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),
+      tcb::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, SequentialGroupInterruptTest) {
+  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),
+      tcb::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, DeadlineGroupNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelDeadlineGroup group{InstantCommand(), InstantCommand()};
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  ParallelDeadlineGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ParallelDeadlineGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}

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

@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ParallelRaceGroup.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ParallelRaceGroupTest : public CommandTestBase {};
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTest) {
+  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{tcb::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, ParallelRaceInterruptTest) {
+  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{tcb::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, ParallelRaceNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  ParallelRaceGroup group{InstantCommand(), InstantCommand()};
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, ParallelRaceCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  ParallelRaceGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(ParallelRaceGroupTest, RaceGroupRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  ParallelRaceGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}

wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/PerpetualCommand.h"
+
+using namespace frc2;
+class PerpetualCommandTest : public CommandTestBase {};
+
+TEST_F(PerpetualCommandTest, PerpetualCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool check = false;
+
+  PerpetualCommand command{InstantCommand([&check] { check = true; }, {})};
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  EXPECT_TRUE(check);
+}

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

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/PrintCommand.h"
+
+using namespace frc2;
+class PrintCommandTest : public CommandTestBase {};
+
+TEST_F(PrintCommandTest, PrintCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  PrintCommand command("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));
+}

wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ProxyScheduleCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class ProxyScheduleCommandTest : public CommandTestBase {};
+
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandScheduleTest) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool scheduled = false;
+
+  InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
+
+  ProxyScheduleCommand command(&toSchedule);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduled);
+}
+
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandEndTest) {
+  CommandScheduler& scheduler = CommandScheduler::GetInstance();
+
+  bool finished = false;
+
+  WaitUntilCommand toSchedule([&finished] { return finished; });
+
+  ProxyScheduleCommand command(&toSchedule);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

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

@@ -0,0 +1,156 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the 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, RobotDisabledCommandCancelTest) {
+  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, RunWhenDisabledTest) {
+  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, SequentialGroupRunWhenDisabledTest) {
+  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, ParallelGroupRunWhenDisabledTest) {
+  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, ParallelRaceRunWhenDisabledTest) {
+  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, ParallelDeadlineRunWhenDisabledTest) {
+  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, ConditionalCommandRunWhenDisabledTest) {
+  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, SelectCommandRunWhenDisabledTest) {
+  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));
+}

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

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class RunCommandTest : public CommandTestBase {};
+
+TEST_F(RunCommandTest, RunCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  RunCommand command([&counter] { counter++; }, {});
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  scheduler.Run();
+  scheduler.Run();
+
+  EXPECT_EQ(3, counter);
+}

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

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <regex>
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/ScheduleCommand.h"
+
+using namespace frc2;
+class ScheduleCommandTest : public CommandTestBase {};
+
+TEST_F(ScheduleCommandTest, ScheduleCommandScheduleTest) {
+  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));
+}

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

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/RunCommand.h"
+
+using namespace frc2;
+class SchedulerTest : public CommandTestBase {};
+
+TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) {
+  CommandScheduler scheduler = GetScheduler();
+
+  InstantCommand command;
+
+  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, SchedulerLambdaInterruptTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  RunCommand command([] {}, {});
+
+  int counter = 0;
+
+  scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  scheduler.Cancel(&command);
+
+  EXPECT_EQ(counter, 1);
+}
+
+TEST_F(SchedulerTest, UnregisterSubsystemTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem system;
+
+  scheduler.RegisterSubsystem(&system);
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system));
+}

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

@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/ConditionalCommand.h"
+#include "frc2/command/InstantCommand.h"
+#include "frc2/command/SelectCommand.h"
+
+using namespace frc2;
+class SelectCommandTest : public CommandTestBase {};
+
+TEST_F(SelectCommandTest, SelectCommandTest) {
+  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, SelectCommandRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  SelectCommand<int> select([] { 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));
+}

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

@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.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, SequentialGroupScheduleTest) {
+  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{tcb::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, SequentialGroupInterruptTest) {
+  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{tcb::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, SequentialGroupNotScheduledCancelTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  SequentialCommandGroup group{InstantCommand(), InstantCommand()};
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupCopyTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  SequentialCommandGroup group(command);
+  scheduler.Schedule(&group);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&group));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}
+
+TEST_F(SequentialCommandGroupTest, SequentialGroupRequirementTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  TestSubsystem requirement1;
+  TestSubsystem requirement2;
+  TestSubsystem requirement3;
+  TestSubsystem requirement4;
+
+  InstantCommand command1([] {}, {&requirement1, &requirement2});
+  InstantCommand command2([] {}, {&requirement3});
+  InstantCommand command3([] {}, {&requirement3, &requirement4});
+
+  SequentialCommandGroup group(std::move(command1), std::move(command2));
+
+  scheduler.Schedule(&group);
+  scheduler.Schedule(&command3);
+
+  EXPECT_TRUE(scheduler.IsScheduled(&command3));
+  EXPECT_FALSE(scheduler.IsScheduled(&group));
+}

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

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/StartEndCommand.h"
+
+using namespace frc2;
+class StartEndCommandTest : public CommandTestBase {};
+
+TEST_F(StartEndCommandTest, StartEndCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  StartEndCommand command([&counter] { counter++; }, [&counter] { counter++; },
+                          {});
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  scheduler.Run();
+  scheduler.Cancel(&command);
+
+  EXPECT_EQ(2, counter);
+}

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

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/WaitCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitCommandTest : public CommandTestBase {};
+
+TEST_F(WaitCommandTest, WaitCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  WaitCommand command(.1);
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  std::this_thread::sleep_for(std::chrono::milliseconds(110));
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

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

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "CommandTestBase.h"
+#include "frc2/command/WaitUntilCommand.h"
+
+using namespace frc2;
+class WaitUntilCommandTest : public CommandTestBase {};
+
+TEST_F(WaitUntilCommandTest, WaitUntilCommandScheduleTest) {
+  CommandScheduler scheduler = GetScheduler();
+
+  bool finished = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  scheduler.Schedule(&command);
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}

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

@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+namespace tcb {
+
+namespace detail {
+
+template <typename T, typename...>
+struct vec_type_helper {
+  using type = T;
+};
+
+template <typename... Args>
+struct vec_type_helper<void, Args...> {
+  using type = typename std::common_type<Args...>::type;
+};
+
+template <typename T, typename... Args>
+using vec_type_helper_t = typename vec_type_helper<T, Args...>::type;
+
+template <typename, typename...>
+struct all_constructible_and_convertible : std::true_type {};
+
+template <typename T, typename First, typename... Rest>
+struct all_constructible_and_convertible<T, First, Rest...>
+    : std::conditional<std::is_constructible<T, First>::value &&
+                           std::is_convertible<First, T>::value,
+                       all_constructible_and_convertible<T, Rest...>,
+                       std::false_type>::type {};
+
+template <typename T, typename... Args,
+          typename std::enable_if<!std::is_trivially_copyable<T>::value,
+                                  int>::type = 0>
+std::vector<T> make_vector_impl(Args&&... args) {
+  std::vector<T> vec;
+  vec.reserve(sizeof...(Args));
+  using arr_t = int[];
+  (void)arr_t{0, (vec.emplace_back(std::forward<Args>(args)), 0)...};
+  return vec;
+}
+
+template <typename T, typename... Args,
+          typename std::enable_if<std::is_trivially_copyable<T>::value,
+                                  int>::type = 0>
+std::vector<T> make_vector_impl(Args&&... args) {
+  return std::vector<T>{std::forward<Args>(args)...};
+}
+
+}  // namespace detail
+
+template <typename T = void, typename... Args,
+          typename V = detail::vec_type_helper_t<T, Args...>,
+          typename std::enable_if<
+              detail::all_constructible_and_convertible<V, Args...>::value,
+              int>::type = 0>
+std::vector<V> make_vector(Args&&... args) {
+  return detail::make_vector_impl<V>(std::forward<Args>(args)...);
+}
+
+}  // namespace tcb