Fix cancel of inner commands in ConditionalCommands (#858)

wpilibcIntegrationTests/src/FRCUserProgram/cpp/command/ConditionalCommandTest.cpp

@@ -10,6 +10,7 @@
 
 #include "Commands/ConditionalCommand.h"
 #include "Commands/Scheduler.h"
+#include "Commands/Subsystem.h"
 #include "command/MockCommand.h"
 #include "command/MockConditionalCommand.h"
 #include "gtest/gtest.h"
@@ -21,15 +22,19 @@ class ConditionalCommandTest : public testing::Test {
   MockConditionalCommand* m_command;
   MockCommand* m_onTrue;
   MockCommand* m_onFalse;
+  MockConditionalCommand* m_commandNull;
+  Subsystem* m_subsystem;
 
  protected:
   void SetUp() override {
     RobotState::SetImplementation(DriverStation::GetInstance());
     Scheduler::GetInstance()->SetEnabled(true);
 
-    m_onTrue = new MockCommand();
-    m_onFalse = new MockCommand();
+    m_subsystem = new Subsystem("MockSubsystem");
+    m_onTrue = new MockCommand(m_subsystem);
+    m_onFalse = new MockCommand(m_subsystem);
     m_command = new MockConditionalCommand(m_onTrue, m_onFalse);
+    m_commandNull = new MockConditionalCommand(m_onTrue, nullptr);
   }
 
   void TearDown() override { delete m_command; }
@@ -53,21 +58,58 @@ class ConditionalCommandTest : public testing::Test {
     EXPECT_EQ(end, command.GetEndCount());
     EXPECT_EQ(interrupted, command.GetInterruptedCount());
   }
+
+  void AssertConditionalCommandState(MockConditionalCommand& command,
+                                     int32_t initialize, int32_t execute,
+                                     int32_t isFinished, int32_t end,
+                                     int32_t interrupted) {
+    EXPECT_EQ(initialize, command.GetInitializeCount());
+    EXPECT_EQ(execute, command.GetExecuteCount());
+    EXPECT_EQ(isFinished, command.GetIsFinishedCount());
+    EXPECT_EQ(end, command.GetEndCount());
+    EXPECT_EQ(interrupted, command.GetInterruptedCount());
+  }
 };
 
 TEST_F(ConditionalCommandTest, OnTrueTest) {
   m_command->SetCondition(true);
 
+  SCOPED_TRACE("1");
   Scheduler::GetInstance()->AddCommand(m_command);
   AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
   Scheduler::GetInstance()->Run();  // init command and select m_onTrue
   AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
   Scheduler::GetInstance()->Run();  // init m_onTrue
   AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
   Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 1, 2, 0, 0);
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
   Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 4, 0, 0);
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onTrue->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
 
   EXPECT_TRUE(m_onTrue->GetInitializeCount() > 0)
       << "Did not initialize the true command\n";
@@ -80,16 +122,42 @@ TEST_F(ConditionalCommandTest, OnTrueTest) {
 TEST_F(ConditionalCommandTest, OnFalseTest) {
   m_command->SetCondition(false);
 
+  SCOPED_TRACE("1");
   Scheduler::GetInstance()->AddCommand(m_command);
   AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
   Scheduler::GetInstance()->Run();  // init command and select m_onTrue
   AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
   Scheduler::GetInstance()->Run();  // init m_onTrue
   AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
   Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onFalse, 1, 1, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
   Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onFalse, 1, 2, 4, 0, 0);
+  AssertCommandState(*m_onFalse, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onFalse->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onFalse, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onFalse, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
 
   EXPECT_TRUE(m_onFalse->GetInitializeCount() > 0)
       << "Did not initialize the false command";
@@ -98,3 +166,272 @@ TEST_F(ConditionalCommandTest, OnFalseTest) {
 
   TeardownScheduler();
 }
+
+TEST_F(ConditionalCommandTest, CancelSubCommandTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onTrue->Cancel();
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, CancelCondCommandTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_command->Cancel();
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 1);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 1);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, OnTrueTwiceTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onTrue->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+
+  m_onTrue->ResetCounters();
+  m_command->ResetCounters();
+  Scheduler::GetInstance()->AddCommand(m_command);
+
+  SCOPED_TRACE("11");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("12");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("13");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("14");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("15");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("16");
+  m_onTrue->SetHasFinished(true);
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+  SCOPED_TRACE("17");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, OnTrueInstantTest) {
+  m_command->SetCondition(true);
+  m_onTrue->SetHasFinished(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 1, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 1, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 1, 0);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, CancelRequiresTest) {
+  m_command->SetCondition(true);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_command);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
+  SCOPED_TRACE("5");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
+  SCOPED_TRACE("6");
+  m_onFalse->Start();
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 0, 0);
+  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 0, 1);
+  SCOPED_TRACE("7");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 1, 3, 3, 0, 1);
+  AssertCommandState(*m_onFalse, 1, 1, 1, 0, 0);
+  AssertConditionalCommandState(*m_command, 1, 4, 4, 0, 1);
+
+  TeardownScheduler();
+}
+
+TEST_F(ConditionalCommandTest, OnFalseNullTest) {
+  m_command->SetCondition(false);
+
+  SCOPED_TRACE("1");
+  Scheduler::GetInstance()->AddCommand(m_commandNull);
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("2");
+  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 0, 0, 0, 0, 0);
+  SCOPED_TRACE("3");
+  Scheduler::GetInstance()->Run();  // init m_onTrue
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 1, 1, 1, 1, 0);
+  SCOPED_TRACE("4");
+  Scheduler::GetInstance()->Run();
+  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
+  AssertConditionalCommandState(*m_commandNull, 1, 1, 1, 1, 0);
+
+  TeardownScheduler();
+}

wpilibcIntegrationTests/src/FRCUserProgram/cpp/command/MockCommand.cpp

@@ -9,6 +9,10 @@
 
 using namespace frc;
 
+MockCommand::MockCommand(Subsystem* subsys) : MockCommand() {
+  Requires(subsys);
+}
+
 MockCommand::MockCommand() {
   m_initializeCount = 0;
   m_executeCount = 0;
@@ -36,3 +40,12 @@ bool MockCommand::IsFinished() {
 void MockCommand::End() { ++m_endCount; }
 
 void MockCommand::Interrupted() { ++m_interruptedCount; }
+
+void MockCommand::ResetCounters() {
+  m_initializeCount = 0;
+  m_executeCount = 0;
+  m_isFinishedCount = 0;
+  m_hasFinished = false;
+  m_endCount = 0;
+  m_interruptedCount = 0;
+}

wpilibcIntegrationTests/src/FRCUserProgram/cpp/command/MockConditionalCommand.cpp

@@ -11,10 +11,47 @@ using namespace frc;
 
 MockConditionalCommand::MockConditionalCommand(MockCommand* onTrue,
                                                MockCommand* onFalse)
-    : ConditionalCommand(onTrue, onFalse) {}
+    : ConditionalCommand(onTrue, onFalse) {
+  m_initializeCount = 0;
+  m_executeCount = 0;
+  m_isFinishedCount = 0;
+  m_endCount = 0;
+  m_interruptedCount = 0;
+}
 
 void MockConditionalCommand::SetCondition(bool condition) {
   m_condition = condition;
 }
 
 bool MockConditionalCommand::Condition() { return m_condition; }
+
+bool MockConditionalCommand::HasInitialized() {
+  return GetInitializeCount() > 0;
+}
+
+bool MockConditionalCommand::HasEnd() { return GetEndCount() > 0; }
+
+bool MockConditionalCommand::HasInterrupted() {
+  return GetInterruptedCount() > 0;
+}
+
+void MockConditionalCommand::Initialize() { ++m_initializeCount; }
+
+void MockConditionalCommand::Execute() { ++m_executeCount; }
+
+bool MockConditionalCommand::IsFinished() {
+  ++m_isFinishedCount;
+  return ConditionalCommand::IsFinished();
+}
+
+void MockConditionalCommand::End() { ++m_endCount; }
+
+void MockConditionalCommand::Interrupted() { ++m_interruptedCount; }
+
+void MockConditionalCommand::ResetCounters() {
+  m_initializeCount = 0;
+  m_executeCount = 0;
+  m_isFinishedCount = 0;
+  m_endCount = 0;
+  m_interruptedCount = 0;
+}

wpilibcIntegrationTests/src/FRCUserProgram/headers/command/MockCommand.h

@@ -13,6 +13,7 @@ namespace frc {
 
 class MockCommand : public Command {
  public:
+  explicit MockCommand(Subsystem*);
   MockCommand();
   int32_t GetInitializeCount() { return m_initializeCount; }
   bool HasInitialized();
@@ -26,6 +27,7 @@ class MockCommand : public Command {
 
   int32_t GetInterruptedCount() { return m_interruptedCount; }
   bool HasInterrupted();
+  void ResetCounters();
 
  protected:
   void Initialize() override;

wpilibcIntegrationTests/src/FRCUserProgram/headers/command/MockConditionalCommand.h

@@ -16,12 +16,33 @@ class MockConditionalCommand : public ConditionalCommand {
  public:
   MockConditionalCommand(MockCommand* onTrue, MockCommand* onFalse);
   void SetCondition(bool condition);
+  int32_t GetInitializeCount() { return m_initializeCount; }
+  bool HasInitialized();
+
+  int32_t GetExecuteCount() { return m_executeCount; }
+  int32_t GetIsFinishedCount() { return m_isFinishedCount; }
+  int32_t GetEndCount() { return m_endCount; }
+  bool HasEnd();
+
+  int32_t GetInterruptedCount() { return m_interruptedCount; }
+  bool HasInterrupted();
+  void ResetCounters();
 
  protected:
   bool Condition() override;
+  void Initialize() override;
+  void Execute() override;
+  bool IsFinished() override;
+  void End() override;
+  void Interrupted() override;
 
  private:
   bool m_condition = false;
+  int32_t m_initializeCount;
+  int32_t m_executeCount;
+  int32_t m_isFinishedCount;
+  int32_t m_endCount;
+  int32_t m_interruptedCount;
 };
 
 }  // namespace frc