Makes FMS data never be a null string. (#900)

Fixes #895

hal/src/main/native/include/HAL/handles/IndexedClassedHandleResource.h

@@ -9,6 +9,7 @@
 
 #include <stdint.h>
 
+#include <array>
 #include <memory>
 #include <vector>
 
@@ -40,7 +41,7 @@ class IndexedClassedHandleResource : public HandleBase {
   friend class IndexedClassedHandleResourceTest;
 
  public:
-  IndexedClassedHandleResource();
+  IndexedClassedHandleResource() = default;
   IndexedClassedHandleResource(const IndexedClassedHandleResource&) = delete;
   IndexedClassedHandleResource& operator=(const IndexedClassedHandleResource&) =
       delete;
@@ -49,20 +50,13 @@ class IndexedClassedHandleResource : public HandleBase {
                    int32_t* status);
   std::shared_ptr<TStruct> Get(THandle handle);
   void Free(THandle handle);
+  void ResetHandles();
 
  private:
-  std::array<std::shared_ptr<TStruct>[], size> m_structures;
+  std::array<std::shared_ptr<TStruct>, size> m_structures;
-  std::array<wpi::mutex[], size> m_handleMutexes;
+  std::array<wpi::mutex, size> m_handleMutexes;
 };
 
-template <typename THandle, typename TStruct, int16_t size,
-          HAL_HandleEnum enumValue>
-IndexedClassedHandleResource<THandle, TStruct, size,
-                             enumValue>::IndexedClassedHandleResource() {
-  m_structures = std::make_unique<std::shared_ptr<TStruct>[]>(size);
-  m_handleMutexes = std::make_unique<wpi::mutex[]>(size);
-}
-
 template <typename THandle, typename TStruct, int16_t size,
           HAL_HandleEnum enumValue>
 THandle

hal/src/test/native/cpp/handles/HandleTests.cpp

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 "HAL/HAL.h"
+#include "HAL/handles/IndexedClassedHandleResource.h"
+#include "gtest/gtest.h"
+
+#define HAL_TestHandle HAL_Handle
+
+namespace {
+class MyTestClass {};
+}  // namespace
+
+namespace hal {
+TEST(HandleTests, ClassedHandleTest) {
+  hal::IndexedClassedHandleResource<HAL_TestHandle, MyTestClass, 8,
+                                    HAL_HandleEnum::Vendor>
+      testClass;
+  int32_t status = 0;
+  testClass.Allocate(0, std::make_unique<MyTestClass>(), &status);
+  EXPECT_EQ(0, status);
+}
+
+}  // namespace hal

wpilibc/src/main/native/cpp/Commands/Command.cpp

@@ -140,6 +140,7 @@ void Command::Removed() {
   m_initialized = false;
   m_canceled = false;
   m_running = false;
+  m_completed = true;
 }
 
 /**
@@ -156,6 +157,7 @@ void Command::Start() {
         CommandIllegalUse,
         "Can not start a command that is part of a command group");
 
+  m_completed = false;
   Scheduler::GetInstance()->AddCommand(this);
 }
 
@@ -211,13 +213,13 @@ void Command::End() {}
  */
 void Command::Interrupted() { End(); }
 
-void Command::_Initialize() {}
+void Command::_Initialize() { m_completed = false; }
 
-void Command::_Interrupted() {}
+void Command::_Interrupted() { m_completed = true; }
 
 void Command::_Execute() {}
 
-void Command::_End() {}
+void Command::_End() { m_completed = true; }
 
 /**
  * Called to indicate that the timer should start.
@@ -318,6 +320,7 @@ void Command::ClearRequirements() { m_requirements.clear(); }
 void Command::StartRunning() {
   m_running = true;
   m_startTime = -1;
+  m_completed = false;
 }
 
 /**
@@ -330,6 +333,20 @@ void Command::StartRunning() {
  */
 bool Command::IsRunning() const { return m_running; }
 
+/**
+ * Returns whether or not the command has been initialized.
+ *
+ * @return whether or not the command has been initialized.
+ */
+bool Command::IsInitialized() const { return m_initialized; }
+
+/**
+ * Returns whether or not the command has completed running.
+ *
+ * @return whether or not the command has completed running.
+ */
+bool Command::IsCompleted() const { return m_completed; }
+
 /**
  * This will cancel the current command.
  *

wpilibc/src/main/native/cpp/Commands/ConditionalCommand.cpp

@@ -7,6 +7,8 @@
 
 #include "Commands/ConditionalCommand.h"
 
+#include <iostream>
+
 #include "Commands/Scheduler.h"
 
 using namespace frc;
@@ -65,6 +67,7 @@ void ConditionalCommand::_Initialize() {
 
     m_chosenCommand->Start();
   }
+  Command::_Initialize();
 }
 
 void ConditionalCommand::_Cancel() {
@@ -76,14 +79,17 @@ void ConditionalCommand::_Cancel() {
 }
 
 bool ConditionalCommand::IsFinished() {
-  return m_chosenCommand != nullptr && m_chosenCommand->IsRunning() &&
+  if (m_chosenCommand != nullptr) {
-         m_chosenCommand->IsFinished();
+    return m_chosenCommand->IsCompleted();
+  } else {
+    return true;
+  }
 }
 
-void ConditionalCommand::Interrupted() {
+void ConditionalCommand::_Interrupted() {
   if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
     m_chosenCommand->Cancel();
   }
 
-  Command::Interrupted();
+  Command::_Interrupted();
 }

wpilibc/src/main/native/cpp/DriverStation.cpp

@@ -13,6 +13,10 @@
 #include <HAL/Power.h>
 #include <HAL/cpp/Log.h>
 #include <llvm/SmallString.h>
+#include <llvm/StringRef.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <networktables/NetworkTableInstance.h>
 
 #include "AnalogInput.h"
 #include "MotorSafetyHelper.h"
@@ -28,6 +32,42 @@ struct MatchInfoData {
   int replayNumber = 0;
   DriverStation::MatchType matchType = DriverStation::MatchType::kNone;
 };
+
+class MatchDataSender {
+ public:
+  std::shared_ptr<nt::NetworkTable> table;
+  nt::NetworkTableEntry typeMetadata;
+  nt::NetworkTableEntry gameSpecificMessage;
+  nt::NetworkTableEntry eventName;
+  nt::NetworkTableEntry matchNumber;
+  nt::NetworkTableEntry replayNumber;
+  nt::NetworkTableEntry matchType;
+  nt::NetworkTableEntry alliance;
+  nt::NetworkTableEntry station;
+  nt::NetworkTableEntry controlWord;
+
+  MatchDataSender() {
+    table = nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo");
+    typeMetadata = table->GetEntry(".type");
+    typeMetadata.ForceSetString("FMSInfo");
+    gameSpecificMessage = table->GetEntry("GameSpecificMessage");
+    gameSpecificMessage.ForceSetString("");
+    eventName = table->GetEntry("EventName");
+    eventName.ForceSetString("");
+    matchNumber = table->GetEntry("MatchNumber");
+    matchNumber.ForceSetDouble(0);
+    replayNumber = table->GetEntry("ReplayNumber");
+    replayNumber.ForceSetDouble(0);
+    matchType = table->GetEntry("MatchType");
+    matchType.ForceSetDouble(0);
+    alliance = table->GetEntry("IsRedAlliance");
+    alliance.ForceSetBoolean(true);
+    station = table->GetEntry("StationNumber");
+    station.ForceSetDouble(1);
+    controlWord = table->GetEntry("FMSControlData");
+    controlWord.ForceSetDouble(0);
+  }
+};
 }  // namespace frc
 
 using namespace frc;
@@ -589,7 +629,22 @@ void DriverStation::WaitForData() { WaitForData(0); }
  * @return true if new data, otherwise false
  */
 bool DriverStation::WaitForData(double timeout) {
-  return static_cast<bool>(HAL_WaitForDSDataTimeout(timeout));
+  auto timeoutTime =
+      std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+
+  std::unique_lock<wpi::mutex> lock(m_waitForDataMutex);
+  int currentCount = m_waitForDataCounter;
+  while (m_waitForDataCounter == currentCount) {
+    if (timeout > 0) {
+      auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime);
+      if (timedOut == std::cv_status::timeout) {
+        return false;
+      }
+    } else {
+      m_waitForDataCond.wait(lock);
+    }
+  }
+  return true;
 }
 
 /**
@@ -625,6 +680,65 @@ double DriverStation::GetBatteryVoltage() const {
   return voltage;
 }
 
+void DriverStation::SendMatchData() {
+  int32_t status = 0;
+  HAL_AllianceStationID alliance = HAL_GetAllianceStation(&status);
+  bool isRedAlliance = false;
+  int stationNumber = 1;
+  switch (alliance) {
+    case HAL_AllianceStationID::HAL_AllianceStationID_kBlue1:
+      isRedAlliance = false;
+      stationNumber = 1;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kBlue2:
+      isRedAlliance = false;
+      stationNumber = 2;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kBlue3:
+      isRedAlliance = false;
+      stationNumber = 3;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kRed1:
+      isRedAlliance = true;
+      stationNumber = 1;
+      break;
+    case HAL_AllianceStationID::HAL_AllianceStationID_kRed2:
+      isRedAlliance = true;
+      stationNumber = 2;
+      break;
+    default:
+      isRedAlliance = true;
+      stationNumber = 3;
+      break;
+  }
+
+  MatchInfoData tmpDataStore;
+  {
+    std::lock_guard<wpi::mutex> lock(m_cacheDataMutex);
+    tmpDataStore = *m_matchInfo;
+  }
+
+  m_matchDataSender->alliance.SetBoolean(isRedAlliance);
+  m_matchDataSender->station.SetDouble(stationNumber);
+  m_matchDataSender->eventName.SetString(tmpDataStore.eventName);
+  m_matchDataSender->gameSpecificMessage.SetString(
+      tmpDataStore.gameSpecificMessage);
+  m_matchDataSender->matchNumber.SetDouble(tmpDataStore.matchNumber);
+  m_matchDataSender->replayNumber.SetDouble(tmpDataStore.replayNumber);
+  m_matchDataSender->matchType.SetDouble(
+      static_cast<int>(tmpDataStore.matchType));
+
+  HAL_ControlWord ctlWord;
+  {
+    // Valid, as in other places we guarentee ctlWord >= int32
+    std::lock_guard<wpi::mutex> lock(m_controlWordMutex);
+    ctlWord = m_controlWordCache;
+  }
+  int32_t wordInt = 0;
+  std::memcpy(&wordInt, &ctlWord, sizeof(wordInt));
+  m_matchDataSender->controlWord.SetDouble(wordInt);
+}
+
 /**
  * Copy data from the DS task for the user.
  *
@@ -655,25 +769,37 @@ void DriverStation::GetData() {
   // Force a control word update, to make sure the data is the newest.
   HAL_ControlWord controlWord;
   UpdateControlWord(true, controlWord);
-  // Obtain a write lock on the data, swap the cached data into the
-  // main data arrays
-  std::lock_guard<wpi::mutex> lock(m_cacheDataMutex);
 
-  for (int32_t i = 0; i < kJoystickPorts; i++) {
+  {
-    // If buttons weren't pressed and are now, set flags in m_buttonsPressed
+    // Obtain a write lock on the data, swap the cached data into the
-    m_joystickButtonsPressed[i] |=
+    // main data arrays
-        ~m_joystickButtons[i].buttons & m_joystickButtonsCache[i].buttons;
+    std::lock_guard<wpi::mutex> lock(m_cacheDataMutex);
 
-    // If buttons were pressed and aren't now, set flags in m_buttonsReleased
+    for (int32_t i = 0; i < kJoystickPorts; i++) {
-    m_joystickButtonsReleased[i] |=
+      // If buttons weren't pressed and are now, set flags in m_buttonsPressed
-        m_joystickButtons[i].buttons & ~m_joystickButtonsCache[i].buttons;
+      m_joystickButtonsPressed[i] |=
+          ~m_joystickButtons[i].buttons & m_joystickButtonsCache[i].buttons;
+
+      // If buttons were pressed and aren't now, set flags in m_buttonsReleased
+      m_joystickButtonsReleased[i] |=
+          m_joystickButtons[i].buttons & ~m_joystickButtonsCache[i].buttons;
+    }
+
+    m_joystickAxes.swap(m_joystickAxesCache);
+    m_joystickPOVs.swap(m_joystickPOVsCache);
+    m_joystickButtons.swap(m_joystickButtonsCache);
+    m_joystickDescriptor.swap(m_joystickDescriptorCache);
+    m_matchInfo.swap(m_matchInfoCache);
   }
 
-  m_joystickAxes.swap(m_joystickAxesCache);
+  {
-  m_joystickPOVs.swap(m_joystickPOVsCache);
+    std::lock_guard<wpi::mutex> waitLock(m_waitForDataMutex);
-  m_joystickButtons.swap(m_joystickButtonsCache);
+    // Nofify all threads
-  m_joystickDescriptor.swap(m_joystickDescriptorCache);
+    m_waitForDataCounter++;
-  m_matchInfo.swap(m_matchInfoCache);
+    m_waitForDataCond.notify_all();
+  }
+
+  SendMatchData();
 }
 
 /**
@@ -682,6 +808,7 @@ void DriverStation::GetData() {
  * This is only called once the first time GetInstance() is called
  */
 DriverStation::DriverStation() {
+  m_waitForDataCounter = 0;
   m_joystickAxes = std::make_unique<HAL_JoystickAxes[]>(kJoystickPorts);
   m_joystickPOVs = std::make_unique<HAL_JoystickPOVs[]>(kJoystickPorts);
   m_joystickButtons = std::make_unique<HAL_JoystickButtons[]>(kJoystickPorts);
@@ -696,6 +823,8 @@ DriverStation::DriverStation() {
       std::make_unique<HAL_JoystickDescriptor[]>(kJoystickPorts);
   m_matchInfoCache = std::make_unique<MatchInfoData>();
 
+  m_matchDataSender = std::make_unique<MatchDataSender>();
+
   // All joysticks should default to having zero axes, povs and buttons, so
   // uninitialized memory doesn't get sent to speed controllers.
   for (unsigned int i = 0; i < kJoystickPorts; i++) {

wpilibc/src/main/native/cpp/PIDController.cpp

@@ -367,8 +367,8 @@ double PIDController::Get() const {
 /**
  * Set the PID controller to consider the input to be continuous,
  *
- * Rather then using the max and min in as constraints, it considers them to
+ * Rather then using the max and min input range as constraints, it considers
- * be the same point and automatically calculates the shortest route to
+ * them to be the same point and automatically calculates the shortest route to
  * the setpoint.
  *
  * @param continuous true turns on continuous, false turns off continuous
@@ -652,7 +652,7 @@ void PIDController::InitSendable(SendableBuilder& builder) {
  * @return Error for continuous inputs.
  */
 double PIDController::GetContinuousError(double error) const {
-  if (m_continuous) {
+  if (m_continuous && m_inputRange != 0) {
     error = std::fmod(error, m_inputRange);
     if (std::fabs(error) > m_inputRange / 2) {
       if (error > 0) {

wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp

@@ -19,7 +19,7 @@ void SpeedControllerGroup::Set(double speed) {
 
 double SpeedControllerGroup::Get() const {
   if (!m_speedControllers.empty()) {
-    return m_speedControllers.front().get().Get();
+    return m_speedControllers.front().get().Get() * (m_isInverted ? -1 : 1);
   }
   return 0.0;
 }
@@ -42,11 +42,7 @@ void SpeedControllerGroup::StopMotor() {
   }
 }
 
-void SpeedControllerGroup::PIDWrite(double output) {
+void SpeedControllerGroup::PIDWrite(double output) { Set(output); }
-  for (auto speedController : m_speedControllers) {
-    speedController.get().PIDWrite(output);
-  }
-}
 
 void SpeedControllerGroup::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Speed Controller");

wpilibc/src/main/native/include/Commands/Command.h

@@ -62,6 +62,8 @@ class Command : public ErrorBase, public SendableBase {
   bool Run();
   void Cancel();
   bool IsRunning() const;
+  bool IsInitialized() const;
+  bool IsCompleted() const;
   bool IsInterruptible() const;
   void SetInterruptible(bool interruptible);
   bool DoesRequire(Subsystem* subsystem) const;
@@ -148,6 +150,9 @@ class Command : public ErrorBase, public SendableBase {
   // The CommandGroup this is in
   CommandGroup* m_parent = nullptr;
 
+  // Whether or not this command has completed running
+  bool m_completed = false;
+
   int m_commandID = m_commandCounter++;
   static int m_commandCounter;
 

wpilibc/src/main/native/include/Commands/ConditionalCommand.h

@@ -10,23 +10,25 @@
 #include <llvm/Twine.h>
 
 #include "Commands/Command.h"
-#include "Commands/InstantCommand.h"
 
 namespace frc {
 
 /**
  * A ConditionalCommand is a Command that starts one of two commands.
  *
- * A ConditionalCommand uses m_condition to determine whether it should run
+ * A ConditionalCommand uses the Condition method to determine whether it should
- * m_onTrue or m_onFalse.
+ * run onTrue or onFalse.
  *
  * A ConditionalCommand adds the proper Command to the Scheduler during
  * Initialize() and then IsFinished() will return true once that Command has
  * finished executing.
  *
- * If no Command is specified for m_onFalse, the occurrence of that condition
+ * If no Command is specified for onFalse, the occurrence of that condition
  * will be a no-op.
  *
+ * A CondtionalCommand will require the superset of subsystems of the onTrue
+ * and onFalse commands.
+ *
  * @see Command
  * @see Scheduler
  */
@@ -48,7 +50,7 @@ class ConditionalCommand : public Command {
   void _Initialize() override;
   void _Cancel() override;
   bool IsFinished() override;
-  void Interrupted() override;
+  void _Interrupted() override;
 
  private:
   // The Command to execute if Condition() returns true

wpilibc/src/main/native/include/DriverStation.h

@@ -15,6 +15,7 @@
 
 #include <HAL/DriverStation.h>
 #include <llvm/Twine.h>
+#include <support/condition_variable.h>
 #include <support/deprecated.h>
 #include <support/mutex.h>
 
@@ -24,6 +25,7 @@
 namespace frc {
 
 struct MatchInfoData;
+class MatchDataSender;
 
 /**
  * Provide access to the network communication data to / from the Driver
@@ -132,6 +134,7 @@ class DriverStation : public ErrorBase, public RobotStateInterface {
   void ReportJoystickUnpluggedWarning(const llvm::Twine& message);
   void Run();
   void UpdateControlWord(bool force, HAL_ControlWord& controlWord) const;
+  void SendMatchData();
 
   // Joystick User Data
   std::unique_ptr<HAL_JoystickAxes[]> m_joystickAxes;
@@ -147,6 +150,8 @@ class DriverStation : public ErrorBase, public RobotStateInterface {
   std::unique_ptr<HAL_JoystickDescriptor[]> m_joystickDescriptorCache;
   std::unique_ptr<MatchInfoData> m_matchInfoCache;
 
+  std::unique_ptr<MatchDataSender> m_matchDataSender;
+
   // Joystick button rising/falling edge flags
   std::array<uint32_t, kJoystickPorts> m_joystickButtonsPressed;
   std::array<uint32_t, kJoystickPorts> m_joystickButtonsReleased;
@@ -155,6 +160,10 @@ class DriverStation : public ErrorBase, public RobotStateInterface {
   std::thread m_dsThread;
   std::atomic<bool> m_isRunning{false};
 
+  wpi::mutex m_waitForDataMutex;
+  wpi::condition_variable m_waitForDataCond;
+  int m_waitForDataCounter;
+
   mutable wpi::mutex m_cacheDataMutex;
 
   // Robot state status variables

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_subsystem = new Subsystem("MockSubsystem");
-    m_onFalse = new MockCommand();
+    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

wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -8,7 +8,14 @@
 package edu.wpi.first.wpilibj;
 
 import java.nio.ByteBuffer;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
 
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.hal.AllianceStationID;
 import edu.wpi.first.wpilibj.hal.ControlWord;
 import edu.wpi.first.wpilibj.hal.HAL;
@@ -73,6 +80,51 @@ public class DriverStation implements RobotState.Interface {
     }
   } /* DriverStationTask */
 
+  private static class MatchDataSender {
+    @SuppressWarnings("MemberName")
+    NetworkTable table;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry typeMetadata;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry gameSpecificMessage;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry eventName;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry matchNumber;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry replayNumber;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry matchType;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry alliance;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry station;
+    @SuppressWarnings("MemberName")
+    NetworkTableEntry controlWord;
+
+    MatchDataSender() {
+      table = NetworkTableInstance.getDefault().getTable("FMSInfo");
+      typeMetadata = table.getEntry(".type");
+      typeMetadata.forceSetString("FMSInfo");
+      gameSpecificMessage = table.getEntry("GameSpecificMessage");
+      gameSpecificMessage.forceSetString("");
+      eventName = table.getEntry("EventName");
+      eventName.forceSetString("");
+      matchNumber = table.getEntry("MatchNumber");
+      matchNumber.forceSetDouble(0);
+      replayNumber = table.getEntry("ReplayNumber");
+      replayNumber.forceSetDouble(0);
+      matchType = table.getEntry("MatchType");
+      matchType.forceSetDouble(0);
+      alliance = table.getEntry("IsRedAlliance");
+      alliance.forceSetBoolean(true);
+      station = table.getEntry("StationNumber");
+      station.forceSetDouble(1);
+      controlWord = table.getEntry("FMSControlData");
+      controlWord.forceSetDouble(0);
+    }
+  }
+
   private static DriverStation instance = new DriverStation();
 
   // Joystick User Data
@@ -94,12 +146,18 @@ public class DriverStation implements RobotState.Interface {
   // preallocated byte buffer for button count
   private ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
 
+  private MatchDataSender m_matchDataSender;
+
   // Internal Driver Station thread
   private Thread m_thread;
   private volatile boolean m_threadKeepAlive = true;
 
   private final Object m_cacheDataMutex;
 
+  private final Lock m_waitForDataMutex;
+  private final Condition m_waitForDataCond;
+  private int m_waitForDataCount;
+
   // Robot state status variables
   private boolean m_userInDisabled = false;
   private boolean m_userInAutonomous = false;
@@ -127,6 +185,10 @@ public class DriverStation implements RobotState.Interface {
    * variable.
    */
   private DriverStation() {
+    m_waitForDataCount = 0;
+    m_waitForDataMutex = new ReentrantLock();
+    m_waitForDataCond = m_waitForDataMutex.newCondition();
+
     m_cacheDataMutex = new Object();
     for (int i = 0; i < kJoystickPorts; i++) {
       m_joystickButtons[i] = new HALJoystickButtons();
@@ -145,6 +207,8 @@ public class DriverStation implements RobotState.Interface {
     m_controlWordCache = new ControlWord();
     m_lastControlWordUpdate = 0;
 
+    m_matchDataSender = new MatchDataSender();
+
     m_thread = new Thread(new DriverStationTask(this), "FRCDriverStation");
     m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);
 
@@ -794,7 +858,38 @@ public class DriverStation implements RobotState.Interface {
    * @return true if there is new data, otherwise false
    */
   public boolean waitForData(double timeout) {
-    return HAL.waitForDSDataTimeout(timeout);
+    long startTime = RobotController.getFPGATime();
+    long timeoutMicros = (long) (timeout * 1000000);
+    m_waitForDataMutex.lock();
+    try {
+      int currentCount = m_waitForDataCount;
+      while (m_waitForDataCount == currentCount) {
+        if (timeout > 0) {
+          long now = RobotController.getFPGATime();
+          if (now < startTime + timeoutMicros) {
+            // We still have time to wait
+            boolean signaled = m_waitForDataCond.await(startTime + timeoutMicros - now,
+                                                TimeUnit.MICROSECONDS);
+            if (!signaled) {
+              // Return false if a timeout happened
+              return false;
+            }
+          } else {
+            // Time has elapsed.
+            return false;
+          }
+        } else {
+          m_waitForDataCond.await();
+        }
+      }
+      // Return true if we have received a proper signal
+      return true;
+    } catch (InterruptedException ex) {
+      // return false on a thread interrupt
+      return false;
+    } finally {
+      m_waitForDataMutex.unlock();
+    }
   }
 
   /**
@@ -865,6 +960,61 @@ public class DriverStation implements RobotState.Interface {
     m_userInTest = entering;
   }
 
+  private void sendMatchData() {
+    AllianceStationID alliance = HAL.getAllianceStation();
+    boolean isRedAlliance = false;
+    int stationNumber = 1;
+    switch (alliance) {
+      case Blue1:
+        isRedAlliance = false;
+        stationNumber = 1;
+        break;
+      case Blue2:
+        isRedAlliance = false;
+        stationNumber = 2;
+        break;
+      case Blue3:
+        isRedAlliance = false;
+        stationNumber = 3;
+        break;
+      case Red1:
+        isRedAlliance = true;
+        stationNumber = 1;
+        break;
+      case Red2:
+        isRedAlliance = true;
+        stationNumber = 2;
+        break;
+      default:
+        isRedAlliance = true;
+        stationNumber = 3;
+        break;
+    }
+
+
+    String eventName;
+    String gameSpecificMessage;
+    int matchNumber;
+    int replayNumber;
+    int matchType;
+    synchronized (m_cacheDataMutex) {
+      eventName = m_matchInfo.eventName;
+      gameSpecificMessage = m_matchInfo.gameSpecificMessage;
+      matchNumber = m_matchInfo.matchNumber;
+      replayNumber = m_matchInfo.replayNumber;
+      matchType = m_matchInfo.matchType;
+    }
+
+    m_matchDataSender.alliance.setBoolean(isRedAlliance);
+    m_matchDataSender.station.setDouble(stationNumber);
+    m_matchDataSender.eventName.setString(eventName);
+    m_matchDataSender.gameSpecificMessage.setString(gameSpecificMessage);
+    m_matchDataSender.matchNumber.setDouble(matchNumber);
+    m_matchDataSender.replayNumber.setDouble(replayNumber);
+    m_matchDataSender.matchType.setDouble(matchType);
+    m_matchDataSender.controlWord.setDouble(HAL.nativeGetControlWord());
+  }
+
   /**
    * Copy data from the DS task for the user. If no new data exists, it will just be returned,
    * otherwise the data will be copied from the DS polling loop.
@@ -914,6 +1064,13 @@ public class DriverStation implements RobotState.Interface {
       m_matchInfo = m_matchInfoCache;
       m_matchInfoCache = currentInfo;
     }
+
+    m_waitForDataMutex.lock();
+    m_waitForDataCount++;
+    m_waitForDataCond.signalAll();
+    m_waitForDataMutex.unlock();
+
+    sendMatchData();
   }
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java

@@ -550,12 +550,15 @@ public class PIDController extends SendableBase implements PIDInterface, Sendabl
 
   /**
    * Set the PID controller to consider the input to be continuous, Rather then using the max and
-   * min in as constraints, it considers them to be the same point and automatically calculates the
+   * min input range as constraints, it considers them to be the same point and automatically
-   * shortest route to the setpoint.
+   * calculates the shortest route to the setpoint.
    *
    * @param continuous Set to true turns on continuous, false turns off continuous
    */
   public void setContinuous(boolean continuous) {
+    if (continuous && m_inputRange <= 0) {
+      throw new RuntimeException("No input range set when calling setContinuous().");
+    }
     m_thisMutex.lock();
     try {
       m_continuous = continuous;
@@ -566,8 +569,8 @@ public class PIDController extends SendableBase implements PIDInterface, Sendabl
 
   /**
    * Set the PID controller to consider the input to be continuous, Rather then using the max and
-   * min in as constraints, it considers them to be the same point and automatically calculates the
+   * min input range as constraints, it considers them to be the same point and automatically
-   * shortest route to the setpoint.
+   * calculates the shortest route to the setpoint.
    */
   public void setContinuous() {
     setContinuous(true);
@@ -891,7 +894,7 @@ public class PIDController extends SendableBase implements PIDInterface, Sendabl
    * @return Error for continuous inputs.
    */
   protected double getContinuousError(double error) {
-    if (m_continuous) {
+    if (m_continuous && m_inputRange > 0) {
       error %= m_inputRange;
       if (Math.abs(error) > m_inputRange / 2) {
         if (error > 0) {

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java

@@ -66,7 +66,7 @@ public final class RobotController {
    *
    * @return The battery voltage in Volts.
    */
-  public double getBatteryVoltage() {
+  public static double getBatteryVoltage() {
     return PowerJNI.getVinVoltage();
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java

@@ -21,7 +21,8 @@ import static java.util.Objects.requireNonNull;
  * function (intended for hand created drive code, such as autonomous) or with the Tank/Arcade
  * functions intended to be used for Operator Control driving.
  *
- * @deprecated Use DifferentialDrive or MecanumDrive classes instead.
+ * @deprecated Use {@link edu.wpi.first.wpilibj.drive.DifferentialDrive}
+ *             or {@link edu.wpi.first.wpilibj.drive.MecanumDrive} classes instead.
  */
 @Deprecated
 public class RobotDrive implements MotorSafety {

wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java

@@ -45,7 +45,7 @@ public class SpeedControllerGroup extends SendableBase implements SpeedControlle
   @Override
   public double get() {
     if (m_speedControllers.length > 0) {
-      return m_speedControllers[0].get();
+      return m_speedControllers[0].get() * (m_isInverted ? -1 : 1);
     }
     return 0.0;
   }
@@ -76,9 +76,7 @@ public class SpeedControllerGroup extends SendableBase implements SpeedControlle
 
   @Override
   public void pidWrite(double output) {
-    for (SpeedController speedController : m_speedControllers) {
+    set(output);
-      speedController.pidWrite(output);
-    }
   }
 
   @Override

wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java

@@ -86,6 +86,11 @@ public abstract class Command extends SendableBase implements Sendable {
    */
   private boolean m_runWhenDisabled = false;
 
+  /**
+   * Whether or not this command has completed running.
+   */
+  private boolean m_completed = false;
+
   /**
    * The {@link CommandGroup} this is in.
    */
@@ -208,6 +213,7 @@ public abstract class Command extends SendableBase implements Sendable {
     m_initialized = false;
     m_canceled = false;
     m_running = false;
+    m_completed = true;
   }
 
   /**
@@ -404,6 +410,7 @@ public abstract class Command extends SendableBase implements Sendable {
           "Can not start a command that is a part of a command group");
     }
     Scheduler.getInstance().add(this);
+    m_completed = false;
   }
 
   /**
@@ -467,6 +474,15 @@ public abstract class Command extends SendableBase implements Sendable {
     return m_canceled;
   }
 
+  /**
+   * Whether or not this command has completed running.
+   *
+   * @return whether or not this command has completed running.
+   */
+  public synchronized boolean isCompleted() {
+    return m_completed;
+  }
+
   /**
    * Returns whether or not this command can be interrupted.
    *

wpilibj/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java

@@ -143,6 +143,7 @@ public abstract class ConditionalCommand extends Command {
 
       m_chosenCommand.start();
     }
+    super._initialize();
   }
 
   @Override
@@ -156,16 +157,19 @@ public abstract class ConditionalCommand extends Command {
 
   @Override
   protected boolean isFinished() {
-    return m_chosenCommand != null && m_chosenCommand.isRunning()
+    if (m_chosenCommand != null) {
-        && m_chosenCommand.isFinished();
+      return m_chosenCommand.isCompleted();
+    } else {
+      return true;
+    }
   }
 
   @Override
-  protected void interrupted() {
+  protected void _interrupted() {
     if (m_chosenCommand != null && m_chosenCommand.isRunning()) {
       m_chosenCommand.cancel();
     }
 
-    super.interrupted();
+    super._interrupted();
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/HAL.java

@@ -52,7 +52,7 @@ public class HAL extends JNIWrapper {
    */
   public static native int report(int resource, int instanceNumber, int context, String feature);
 
-  private static native int nativeGetControlWord();
+  public static native int nativeGetControlWord();
 
   @SuppressWarnings("JavadocMethod")
   public static void getControlWord(ControlWord controlWord) {

wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/MatchInfoData.java

@@ -15,13 +15,13 @@ public class MatchInfoData {
    * Stores the event name.
    */
   @SuppressWarnings("MemberName")
-  public String eventName;
+  public String eventName = "";
 
   /**
    * Stores the game specific message.
    */
   @SuppressWarnings("MemberName")
-  public String gameSpecificMessage;
+  public String gameSpecificMessage = "";
 
   /**
    * Stores the match number.

wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java

@@ -7,22 +7,38 @@
 
 package edu.wpi.first.wpilibj.command;
 
+import static org.junit.Assert.assertEquals;
 import static org.junit.Assert.assertTrue;
 
 import org.junit.Before;
+//import org.junit.Ignore;
 import org.junit.Test;
 
 public class ConditionalCommandTest extends AbstractCommandTest {
   MockConditionalCommand m_command;
+  MockConditionalCommand m_commandNull;
   MockCommand m_onTrue;
   MockCommand m_onFalse;
+  MockSubsystem m_subsys;
   Boolean m_condition;
 
   @Before
   public void initCommands() {
-    m_onTrue = new MockCommand();
+    m_subsys = new MockSubsystem();
-    m_onFalse = new MockCommand();
+    m_onTrue = new MockCommand(m_subsys);
+    m_onFalse = new MockCommand(m_subsys);
     m_command = new MockConditionalCommand(m_onTrue, m_onFalse);
+    m_commandNull = new MockConditionalCommand(m_onTrue, null);
+  }
+
+  protected void assertConditionalCommandState(MockConditionalCommand command, int initialize,
+                                               int execute, int isFinished, int end,
+                                               int interrupted) {
+    assertEquals(initialize, command.getInitializeCount());
+    assertEquals(execute, command.getExecuteCount());
+    assertEquals(isFinished, command.getIsFinishedCount());
+    assertEquals(end, command.getEndCount());
+    assertEquals(interrupted, command.getInterruptedCount());
   }
 
   @Test
@@ -31,14 +47,33 @@ public class ConditionalCommandTest extends AbstractCommandTest {
 
     Scheduler.getInstance().add(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);
     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);
     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);
     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);
     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);
+    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, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
 
     assertTrue("Did not initialize the true command", m_onTrue.getInitializeCount() > 0);
     assertTrue("Initialized the false command", m_onFalse.getInitializeCount() == 0);
@@ -50,16 +85,261 @@ public class ConditionalCommandTest extends AbstractCommandTest {
 
     Scheduler.getInstance().add(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);
     Scheduler.getInstance().run();  // init command and select m_onFalse
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
     Scheduler.getInstance().run();  // init m_onFalse
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
     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);
     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);
+    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, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onFalse, 1, 3, 3, 1, 0);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
 
     assertTrue("Did not initialize the false command", m_onFalse.getInitializeCount() > 0);
     assertTrue("Initialized the true command", m_onTrue.getInitializeCount() == 0);
   }
+
+  @Test
+  public void testCancelSubCommand() {
+    m_command.setCondition(true);
+
+    Scheduler.getInstance().add(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);
+    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);
+    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);
+    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);
+    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);
+    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, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 2, 2, 0, 1);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+  }
+
+  @Test
+  public void testCancelRequires() {
+    m_command.setCondition(true);
+
+    Scheduler.getInstance().add(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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+  }
+
+  @Test
+  public void testCancelCondCommand() {
+    m_command.setCondition(true);
+
+    Scheduler.getInstance().add(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);
+    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);
+    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);
+    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);
+    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);
+    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);
+    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);
+  }
+
+  @Test
+  public void testOnTrueTwice() {
+    m_command.setCondition(true);
+
+    Scheduler.getInstance().add(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);
+    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);
+    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);
+    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);
+    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);
+    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, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+
+    m_onTrue.resetCounters();
+    m_command.resetCounters();
+    m_command.setCondition(true);
+    Scheduler.getInstance().add(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);
+    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);
+    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);
+    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);
+    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);
+    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, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 3, 3, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 5, 5, 1, 0);
+  }
+
+  @Test
+  public void testOnTrueInstant() {
+    m_command.setCondition(true);
+    m_onTrue.setHasFinished(true);
+
+    Scheduler.getInstance().add(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);
+    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);
+    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);
+    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, 0, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 1, 1, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 1, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 1, 1, 1, 1, 0);
+    assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_command, 1, 3, 3, 1, 0);
+  }
+
+  @Test
+  public void testOnFalseNull() {
+    m_commandNull.setCondition(false);
+
+    Scheduler.getInstance().add(m_commandNull);
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_commandNull, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init command and select m_onFalse
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_commandNull, 0, 0, 0, 0, 0);
+    Scheduler.getInstance().run();  // init m_onFalse
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_commandNull, 1, 1, 1, 1, 0);
+    Scheduler.getInstance().run();
+    assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
+    assertConditionalCommandState(m_commandNull, 1, 1, 1, 1, 0);
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java

@@ -8,7 +8,7 @@
 package edu.wpi.first.wpilibj.command;
 
 /**
- * A class to simulate a simple command The command keeps track of how many times each method was
+ * A class to simulate a simple command. The command keeps track of how many times each method was
  * called.
  */
 public class MockCommand extends Command {
@@ -19,6 +19,15 @@ public class MockCommand extends Command {
   private int m_endCount = 0;
   private int m_interruptedCount = 0;
 
+  public MockCommand(Subsystem subsys) {
+    super();
+    requires(subsys);
+  }
+
+  public MockCommand() {
+    super();
+  }
+
   protected void initialize() {
     ++m_initializeCount;
   }
@@ -115,4 +124,16 @@ public class MockCommand extends Command {
     return getInterruptedCount() > 0;
   }
 
+  /**
+   * Reset internal counters.
+   */
+  public void resetCounters() {
+    m_initializeCount = 0;
+    m_executeCount = 0;
+    m_isFinishedCount = 0;
+    m_hasFinished = false;
+    m_endCount = 0;
+    m_interruptedCount = 0;
+  }
+
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java

@@ -9,6 +9,11 @@ package edu.wpi.first.wpilibj.command;
 
 public class MockConditionalCommand extends ConditionalCommand {
   private boolean m_condition = false;
+  private int m_initializeCount = 0;
+  private int m_executeCount = 0;
+  private int m_isFinishedCount = 0;
+  private int m_endCount = 0;
+  private int m_interruptedCount = 0;
 
   public MockConditionalCommand(MockCommand onTrue, MockCommand onFalse) {
     super(onTrue, onFalse);
@@ -22,4 +27,94 @@ public class MockConditionalCommand extends ConditionalCommand {
   public void setCondition(boolean condition) {
     this.m_condition = condition;
   }
+
+  protected void initialize() {
+    ++m_initializeCount;
+  }
+
+  protected void execute() {
+    ++m_executeCount;
+  }
+
+  protected boolean isFinished() {
+    ++m_isFinishedCount;
+    return super.isFinished();
+  }
+
+  protected void end() {
+    ++m_endCount;
+  }
+
+  protected void interrupted() {
+    ++m_interruptedCount;
+  }
+
+
+  /**
+   * How many times the initialize method has been called.
+   */
+  public int getInitializeCount() {
+    return m_initializeCount;
+  }
+
+  /**
+   * If the initialize method has been called at least once.
+   */
+  public boolean hasInitialized() {
+    return getInitializeCount() > 0;
+  }
+
+  /**
+   * How many time the execute method has been called.
+   */
+  public int getExecuteCount() {
+    return m_executeCount;
+  }
+
+  /**
+   * How many times the isFinished method has been called.
+   */
+  public int getIsFinishedCount() {
+    return m_isFinishedCount;
+  }
+
+  /**
+   * How many times the end method has been called.
+   */
+  public int getEndCount() {
+    return m_endCount;
+  }
+
+  /**
+   * If the end method has been called at least once.
+   */
+  public boolean hasEnd() {
+    return getEndCount() > 0;
+  }
+
+  /**
+   * How many times the interrupted method has been called.
+   */
+  public int getInterruptedCount() {
+    return m_interruptedCount;
+  }
+
+  /**
+   * If the interrupted method has been called at least once.
+   */
+  public boolean hasInterrupted() {
+    return getInterruptedCount() > 0;
+  }
+
+  /**
+   * Reset internal counters.
+   */
+  public void resetCounters() {
+    m_condition = false;
+    m_initializeCount = 0;
+    m_executeCount = 0;
+    m_isFinishedCount = 0;
+    m_endCount = 0;
+    m_interruptedCount = 0;
+  }
 }

wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java

@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017-2018 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * A class to simulate a simple subsystem.
+ */
+public class MockSubsystem extends Subsystem {
+  protected void initDefaultCommand() {}
+}