Improved Command-based examples (#906)

Robot.cpp has been split into header-source pairs and the Command-based
examples now define an example subsystem in the Robot class.

.travis.yml

@@ -9,7 +9,7 @@ addons:
       - ubuntu-toolchain-r-test
     packages:
       - g++-6
-      - python3.5
+      - python3.5-dev
 
 before_install:
   - sudo sh -c 'echo "deb http://apt.llvm.org/trusty/ llvm-toolchain-trusty-5.0 main" > /etc/apt/sources.list.d/llvm.list'

hal/src/main/native/athena/Notifier.cpp

@@ -169,13 +169,17 @@ void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
     // the notifier can call back into our callback, so don't hold the lock
     // here (the atomic fetch_sub will prevent multiple parallel entries
     // into this function)
-    if (notifierAlarm) notifierAlarm->writeEnable(false, status);
-    if (notifierManager) notifierManager->disable(status);
 
-    std::lock_guard<wpi::mutex> lock(notifierMutex);
-    notifierAlarm = nullptr;
-    notifierManager = nullptr;
-    closestTrigger = UINT64_MAX;
+    // Cleaning up the manager takes up to a second to complete, so don't do
+    // that here. Fix it more permanently in 2019...
+
+    // if (notifierAlarm) notifierAlarm->writeEnable(false, status);
+    // if (notifierManager) notifierManager->disable(status);
+
+    // std::lock_guard<wpi::mutex> lock(notifierMutex);
+    // notifierAlarm = nullptr;
+    // notifierManager = nullptr;
+    // closestTrigger = UINT64_MAX;
   }
 }
 

hal/src/main/native/athena/PWM.cpp

@@ -104,6 +104,9 @@ HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
   digitalSystem->writeEnableMXPSpecialFunction(specialFunctions | bitToSet,
                                                status);
 
+  // Defaults to allow an always valid config.
+  HAL_SetPWMConfig(handle, 2.0, 1.501, 1.5, 1.499, 1.0, status);
+
   return handle;
 }
 void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle, int32_t* status) {

hal/src/main/native/athena/SerialPort.cpp

@@ -43,6 +43,13 @@ void HAL_InitializeSerialPort(HAL_SerialPort port, int32_t* status) {
   if (*status > 0) *status = 0;
 }
 
+void HAL_InitializeSerialPortDirect(HAL_SerialPort port, const char* portName,
+                                    int32_t* status) {
+  *status = viOpen(resourceManagerHandle, const_cast<char*>(portName), VI_NULL,
+                   VI_NULL, reinterpret_cast<ViSession*>(&portHandles[port]));
+  if (*status > 0) *status = 0;
+}
+
 void HAL_SetSerialBaudRate(HAL_SerialPort port, int32_t baud, int32_t* status) {
   *status = viSetAttribute(portHandles[port], VI_ATTR_ASRL_BAUD, baud);
   if (*status > 0) *status = 0;

hal/src/main/native/athena/ctre/CtreCanNode.cpp

@@ -68,17 +68,13 @@ void CtreCanNode::UnregisterTx(uint32_t arbId)
 		_txJobs.erase(iter);
 	}
 }
-timespec diff(const timespec & start, const timespec & end)
-{
-	timespec temp;
-	if ((end.tv_nsec-start.tv_nsec)<0) {
-		temp.tv_sec = end.tv_sec-start.tv_sec-1;
-		temp.tv_nsec = 1000000000+end.tv_nsec-start.tv_nsec;
-	} else {
-		temp.tv_sec = end.tv_sec-start.tv_sec;
-		temp.tv_nsec = end.tv_nsec-start.tv_nsec;
-	}
-	return temp;
+static int64_t GetTimeMs() {
+	std::chrono::time_point < std::chrono::system_clock > now;
+	now = std::chrono::system_clock::now();
+	auto duration = now.time_since_epoch();
+	auto millis = std::chrono::duration_cast < std::chrono::milliseconds
+					> (duration).count();
+	return (int64_t) millis;
 }
 CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeoutMs)
 {
@@ -90,10 +86,11 @@ CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeout
 	if(timeoutMs > 999)
 		timeoutMs = 999;
 	FRC_NetworkCommunication_CANSessionMux_receiveMessage(&arbId,kFullMessageIDMask,dataBytes,&len,&timeStamp,&status);
+	std::lock_guard<wpi::mutex> lock(_lck);
 	if(status == 0){
 		/* fresh update */
 		rxEvent_t & r = _rxRxEvents[arbId]; /* lookup entry or make a default new one with all zeroes */
-		clock_gettime(2,&r.time); 			/* fill in time */
+		r.time = GetTimeMs();
 		memcpy(r.bytes,  dataBytes,  8);	/* fill in databytes */
 	}else{
 		/* did not get the message */
@@ -107,16 +104,13 @@ CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeout
 			/* we've gotten this message before but not recently */
 			memcpy(dataBytes,i->second.bytes,8);
 			/* get the time now */
-			struct timespec temp;
-			clock_gettime(2,&temp); /* get now */
+			int64_t now = GetTimeMs(); /* get now */
 			/* how long has it been? */
-			temp = diff(i->second.time,temp); /* temp = now - last */
-			if(temp.tv_sec > 0){
-				retval = CTR_RxTimeout;
-			}else if(temp.tv_nsec > ((int32_t)timeoutMs*1000*1000)){
-				retval = CTR_RxTimeout;
-			}else {
-				/* our last update was recent enough */
+			int64_t temp = now - i->second.time; /* temp = now - last */
+			if (temp > ((int64_t) timeoutMs)) {
+					retval = CTR_RxTimeout;
+			} else {
+					/* our last update was recent enough */
 			}
 		}
 	}

hal/src/main/native/athena/ctre/CtreCanNode.h

@@ -4,6 +4,7 @@
 #include <map>
 #include <string.h> // memcpy
 #include <sys/time.h>
+#include <support/mutex.h>
 class CtreCanNode
 {
 public:
@@ -108,7 +109,7 @@ private:
 	class rxEvent_t{
 		public:
 			uint8_t bytes[8];
-			struct timespec time;
+			int64_t time;
 			rxEvent_t()
 			{
 				bytes[0] = 0;
@@ -127,5 +128,7 @@ private:
 
 	typedef std::map<uint32_t,rxEvent_t> rxRxEvents_t;
 	rxRxEvents_t _rxRxEvents;
+
+	wpi::mutex _lck;
 };
 #endif

hal/src/main/native/include/HAL/SerialPort.h

@@ -21,6 +21,8 @@ extern "C" {
 #endif
 
 void HAL_InitializeSerialPort(HAL_SerialPort port, int32_t* status);
+void HAL_InitializeSerialPortDirect(HAL_SerialPort port, const char* portName,
+                                    int32_t* status);
 void HAL_SetSerialBaudRate(HAL_SerialPort port, int32_t baud, int32_t* status);
 void HAL_SetSerialDataBits(HAL_SerialPort port, int32_t bits, int32_t* status);
 void HAL_SetSerialParity(HAL_SerialPort port, int32_t parity, int32_t* status);

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<wpi::mutex[], size> m_handleMutexes;
+  std::array<std::shared_ptr<TStruct>, size> m_structures;
+  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/main/native/sim/DIO.cpp

@@ -198,6 +198,23 @@ void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
   SimDIOData[port->channel].SetValue(value);
 }
 
+/**
+ * Set direction of a DIO channel.
+ *
+ * @param channel The Digital I/O channel
+ * @param input true to set input, false for output
+ */
+void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
+                         int32_t* status) {
+  auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  SimDIOData[port->channel].SetIsInput(input);
+}
+
 /**
  * Read a digital I/O bit from the FPGA.
  * Get a single value from a digital I/O channel.

hal/src/main/native/sim/PWM.cpp

@@ -57,6 +57,9 @@ HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
 
   SimPWMData[origChannel].SetInitialized(true);
 
+  // Defaults to allow an always valid config.
+  HAL_SetPWMConfig(handle, 2.0, 1.501, 1.5, 1.499, 1.0, status);
+
   return handle;
 }
 void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle, int32_t* status) {

hal/src/main/native/sim/SerialPort.cpp

@@ -16,6 +16,9 @@ void InitializeSerialPort() {}
 extern "C" {
 void HAL_InitializeSerialPort(HAL_SerialPort port, int32_t* status) {}
 
+void HAL_InitializeSerialPortDirect(HAL_SerialPort port, const char* portName,
+                                    int32_t* status) {}
+
 void HAL_SetSerialBaudRate(HAL_SerialPort port, int32_t baud, int32_t* status) {
 }
 

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() &&
-         m_chosenCommand->IsFinished();
+  if (m_chosenCommand != nullptr) {
+    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/Drive/DifferentialDrive.cpp

@@ -7,6 +7,7 @@
 
 #include "Drive/DifferentialDrive.h"
 
+#include <algorithm>
 #include <cmath>
 
 #include <HAL/HAL.h>
@@ -171,6 +172,14 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
     }
   }
 
+  // Normalize the wheel speeds
+  double maxMagnitude =
+      std::max(std::abs(leftMotorOutput), std::abs(rightMotorOutput));
+  if (maxMagnitude > 1.0) {
+    leftMotorOutput /= maxMagnitude;
+    rightMotorOutput /= maxMagnitude;
+  }
+
   m_leftMotor.Set(leftMotorOutput * m_maxOutput);
   m_rightMotor.Set(-rightMotorOutput * m_maxOutput);
 

wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp

@@ -134,12 +134,12 @@ void MecanumDrive::InitSendable(SendableBuilder& builder) {
                             [=]() { return m_frontLeftMotor.Get(); },
                             [=](double value) { m_frontLeftMotor.Set(value); });
   builder.AddDoubleProperty(
-      "Front Right Motor Speed", [=]() { return m_frontRightMotor.Get(); },
-      [=](double value) { m_frontRightMotor.Set(value); });
+      "Front Right Motor Speed", [=]() { return -m_frontRightMotor.Get(); },
+      [=](double value) { m_frontRightMotor.Set(-value); });
   builder.AddDoubleProperty("Rear Left Motor Speed",
                             [=]() { return m_rearLeftMotor.Get(); },
                             [=](double value) { m_rearLeftMotor.Set(value); });
-  builder.AddDoubleProperty("Rear Right Motor Speed",
-                            [=]() { return m_rearRightMotor.Get(); },
-                            [=](double value) { m_rearRightMotor.Set(value); });
+  builder.AddDoubleProperty(
+      "Rear Right Motor Speed", [=]() { return -m_rearRightMotor.Get(); },
+      [=](double value) { m_rearRightMotor.Set(-value); });
 }

wpilibc/src/main/native/cpp/Drive/RobotDriveBase.cpp

@@ -21,9 +21,11 @@ using namespace frc;
 RobotDriveBase::RobotDriveBase() { m_safetyHelper.SetSafetyEnabled(true); }
 
 /**
- * Change the default value for deadband scaling. The default value is
- * 0.02. Values smaller then the deadband are set to 0, while values
- * larger then the deadband are scaled from 0.0 to 1.0. See ApplyDeadband().
+ * Sets the deadband applied to the drive inputs (e.g., joystick values).
+ *
+ * The default value is 0.02. Inputs smaller than the deadband are set to 0.0
+ * while inputs larger than the deadband are scaled from 0.0 to 1.0. See
+ * ApplyDeadband().
  *
  * @param deadband The deadband to set.
  */

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
-    m_joystickButtonsPressed[i] |=
-        ~m_joystickButtons[i].buttons & m_joystickButtonsCache[i].buttons;
+  {
+    // Obtain a write lock on the data, swap the cached data into the
+    // main data arrays
+    std::lock_guard<wpi::mutex> lock(m_cacheDataMutex);
 
-    // If buttons were pressed and aren't now, set flags in m_buttonsReleased
-    m_joystickButtonsReleased[i] |=
-        m_joystickButtons[i].buttons & ~m_joystickButtonsCache[i].buttons;
+    for (int32_t i = 0; i < kJoystickPorts; i++) {
+      // If buttons weren't pressed and are now, set flags in m_buttonsPressed
+      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);
-  m_joystickButtons.swap(m_joystickButtonsCache);
-  m_joystickDescriptor.swap(m_joystickDescriptorCache);
-  m_matchInfo.swap(m_matchInfoCache);
+  {
+    std::lock_guard<wpi::mutex> waitLock(m_waitForDataMutex);
+    // Nofify all threads
+    m_waitForDataCounter++;
+    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/Joystick.cpp

@@ -131,7 +131,7 @@ int Joystick::GetThrottleChannel() const { return m_axes[Axis::kThrottle]; }
  *             here to complete the GenericHID interface.
  */
 double Joystick::GetX(JoystickHand hand) const {
-  return GetRawAxis(m_axes[kDefaultXAxis]);
+  return GetRawAxis(m_axes[Axis::kX]);
 }
 
 /**
@@ -143,7 +143,7 @@ double Joystick::GetX(JoystickHand hand) const {
  *             here to complete the GenericHID interface.
  */
 double Joystick::GetY(JoystickHand hand) const {
-  return GetRawAxis(m_axes[kDefaultYAxis]);
+  return GetRawAxis(m_axes[Axis::kY]);
 }
 
 /**
@@ -151,16 +151,14 @@ double Joystick::GetY(JoystickHand hand) const {
  *
  * This depends on the mapping of the joystick connected to the current port.
  */
-double Joystick::GetZ() const { return GetRawAxis(m_axes[kDefaultZAxis]); }
+double Joystick::GetZ() const { return GetRawAxis(m_axes[Axis::kZ]); }
 
 /**
  * Get the twist value of the current joystick.
  *
  * This depends on the mapping of the joystick connected to the current port.
  */
-double Joystick::GetTwist() const {
-  return GetRawAxis(m_axes[kDefaultTwistAxis]);
-}
+double Joystick::GetTwist() const { return GetRawAxis(m_axes[Axis::kTwist]); }
 
 /**
  * Get the throttle value of the current joystick.
@@ -168,7 +166,7 @@ double Joystick::GetTwist() const {
  * This depends on the mapping of the joystick connected to the current port.
  */
 double Joystick::GetThrottle() const {
-  return GetRawAxis(m_axes[kDefaultThrottleAxis]);
+  return GetRawAxis(m_axes[Axis::kThrottle]);
 }
 
 /**

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
- * be the same point and automatically calculates the shortest route to
+ * Rather then using the max and min input range as constraints, it considers
+ * 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/PWM.cpp

@@ -311,8 +311,8 @@ void PWM::SetZeroLatch() {
 }
 
 void PWM::InitSendable(SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Speed Controller");
+  builder.SetSmartDashboardType("PWM");
   builder.SetSafeState([=]() { SetDisabled(); });
-  builder.AddDoubleProperty("Value", [=]() { return GetSpeed(); },
-                            [=](double value) { SetSpeed(value); });
+  builder.AddDoubleProperty("Value", [=]() { return GetRaw(); },
+                            [=](double value) { SetRaw(value); });
 }

wpilibc/src/main/native/cpp/PWMSpeedController.cpp

@@ -7,6 +7,8 @@
 
 #include "PWMSpeedController.h"
 
+#include "SmartDashboard/SendableBuilder.h"
+
 using namespace frc;
 
 /**
@@ -52,3 +54,10 @@ void PWMSpeedController::StopMotor() { SafePWM::StopMotor(); }
  * @param output Write out the PWM value as was found in the PIDController
  */
 void PWMSpeedController::PIDWrite(double output) { Set(output); }
+
+void PWMSpeedController::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Speed Controller");
+  builder.SetSafeState([=]() { SetDisabled(); });
+  builder.AddDoubleProperty("Value", [=]() { return GetSpeed(); },
+                            [=](double value) { SetSpeed(value); });
+}

wpilibc/src/main/native/cpp/SerialPort.cpp

@@ -61,6 +61,57 @@ SerialPort::SerialPort(int baudRate, Port port, int dataBits,
   HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
 }
 
+/**
+ * Create an instance of a Serial Port class.
+ *
+ * @param baudRate The baud rate to configure the serial port.
+ * @param port     The physical port to use
+ * @param portName The direct port name to use
+ * @param dataBits The number of data bits per transfer.  Valid values are
+ *                 between 5 and 8 bits.
+ * @param parity   Select the type of parity checking to use.
+ * @param stopBits The number of stop bits to use as defined by the enum
+ *                 StopBits.
+ */
+SerialPort::SerialPort(int baudRate, llvm::StringRef portName, Port port,
+                       int dataBits, SerialPort::Parity parity,
+                       SerialPort::StopBits stopBits) {
+  int32_t status = 0;
+
+  m_port = port;
+
+  llvm::SmallVector<char, 64> buf;
+  const char* portNameC = portName.c_str(buf);
+
+  HAL_InitializeSerialPortDirect(static_cast<HAL_SerialPort>(port), portNameC,
+                                 &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  // Don't continue if initialization failed
+  if (status < 0) return;
+  HAL_SetSerialBaudRate(static_cast<HAL_SerialPort>(port), baudRate, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  HAL_SetSerialDataBits(static_cast<HAL_SerialPort>(port), dataBits, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  HAL_SetSerialParity(static_cast<HAL_SerialPort>(port), parity, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  HAL_SetSerialStopBits(static_cast<HAL_SerialPort>(port), stopBits, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+
+  // Set the default timeout to 5 seconds.
+  SetTimeout(5.0);
+
+  // Don't wait until the buffer is full to transmit.
+  SetWriteBufferMode(kFlushOnAccess);
+
+  EnableTermination();
+
+  // viInstallHandler(m_portHandle, VI_EVENT_IO_COMPLETION, ioCompleteHandler,
+  // this);
+  // viEnableEvent(m_portHandle, VI_EVENT_IO_COMPLETION, VI_HNDLR, VI_NULL);
+
+  HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
+}
+
 /**
  * Destructor.
  */

wpilibc/src/main/native/cpp/SmartDashboard/SendableChooserBase.cpp

@@ -12,3 +12,8 @@ using namespace frc;
 const char* SendableChooserBase::kDefault = "default";
 const char* SendableChooserBase::kOptions = "options";
 const char* SendableChooserBase::kSelected = "selected";
+
+/**
+ * Instantiates a SendableChooser.
+ */
+SendableChooserBase::SendableChooserBase() : SendableBase(false) {}

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) {
-  for (auto speedController : m_speedControllers) {
-    speedController.get().PIDWrite(output);
-  }
-}
+void SpeedControllerGroup::PIDWrite(double output) { Set(output); }
 
 void SpeedControllerGroup::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Speed Controller");

wpilibc/src/main/native/cpp/TimedRobot.cpp

@@ -43,10 +43,15 @@ void TimedRobot::SetPeriod(double period) {
   m_period = period;
 
   if (m_startLoop) {
-    m_loop->StartPeriodic(m_period);
+    m_loop->StartPeriodic(period);
   }
 }
 
+/**
+ * Get time period between calls to Periodic() functions.
+ */
+double TimedRobot::GetPeriod() const { return m_period; }
+
 TimedRobot::TimedRobot() {
   m_loop = std::make_unique<Notifier>(&TimedRobot::LoopFunc, this);
 

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
- * m_onTrue or m_onFalse.
+ * A ConditionalCommand uses the Condition method to determine whether it should
+ * 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

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

@@ -28,6 +28,7 @@ class PWMSpeedController : public SafePWM, public SpeedController {
 
  protected:
   explicit PWMSpeedController(int channel);
+  void InitSendable(SendableBuilder& builder) override;
 
  private:
   bool m_isInverted = false;

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

@@ -10,6 +10,7 @@
 #include <string>
 
 #include <llvm/StringRef.h>
+#include <support/deprecated.h>
 
 #include "ErrorBase.h"
 
@@ -56,6 +57,10 @@ class SerialPort : public ErrorBase {
 
   SerialPort(int baudRate, Port port = kOnboard, int dataBits = 8,
              Parity parity = kParity_None, StopBits stopBits = kStopBits_One);
+  WPI_DEPRECATED("Will be removed for 2019")
+  SerialPort(int baudRate, llvm::StringRef portName, Port port = kOnboard,
+             int dataBits = 8, Parity parity = kParity_None,
+             StopBits stopBits = kStopBits_One);
   ~SerialPort();
 
   SerialPort(const SerialPort&) = delete;

wpilibc/src/main/native/include/SmartDashboard/SendableChooser.inc

@@ -62,7 +62,7 @@ void SendableChooser<T>::AddDefault(llvm::StringRef name, T object) {
 template <class T>
 auto SendableChooser<T>::GetSelected()
     -> decltype(_unwrap_smart_ptr(m_choices[""])) {
-  llvm::StringRef selected = m_defaultChoice;
+  std::string selected = m_defaultChoice;
   if (m_selectedEntry) {
     selected = m_selectedEntry.GetString(m_defaultChoice);
   }

wpilibc/src/main/native/include/SmartDashboard/SendableChooserBase.h

@@ -23,6 +23,7 @@ namespace frc {
  */
 class SendableChooserBase : public SendableBase {
  public:
+  SendableChooserBase();
   ~SendableChooserBase() override = default;
 
  protected:

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

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <atomic>
 #include <memory>
 
 #include "IterativeRobotBase.h"
@@ -30,13 +31,14 @@ class TimedRobot : public IterativeRobotBase {
   void StartCompetition() override;
 
   void SetPeriod(double seconds);
+  double GetPeriod() const;
 
  protected:
   TimedRobot();
   virtual ~TimedRobot();
 
  private:
-  double m_period = kDefaultPeriod;
+  std::atomic<double> m_period{kDefaultPeriod};
 
   // Prevents loop from starting if user calls SetPeriod() in RobotInit()
   bool m_startLoop = false;

wpilibcExamples/src/main/cpp/templates/commandbased/Commands/ExampleCommand.cpp

@@ -7,9 +7,11 @@
 
 #include "ExampleCommand.h"
 
+#include "../Robot.h"
+
 ExampleCommand::ExampleCommand() {
 	// Use Requires() here to declare subsystem dependencies
-	// eg. Requires(&Robot::chassis);
+	Requires(&Robot::m_subsystem);
 }
 
 // Called just before this Command runs the first time

wpilibcExamples/src/main/cpp/templates/commandbased/Commands/MyAutoCommand.cpp

@@ -7,9 +7,11 @@
 
 #include "MyAutoCommand.h"
 
+#include "../Robot.h"
+
 MyAutoCommand::MyAutoCommand() {
 	// Use Requires() here to declare subsystem dependencies
-	// eg. Requires(&Robot::chassis);
+	Requires(&Robot::m_subsystem);
 }
 
 // Called just before this Command runs the first time

wpilibcExamples/src/main/cpp/templates/commandbased/Robot.cpp

@@ -5,93 +5,77 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <Commands/Command.h>
+#include "Robot.h"
+
 #include <Commands/Scheduler.h>
-#include <LiveWindow/LiveWindow.h>
-#include <SmartDashboard/SendableChooser.h>
 #include <SmartDashboard/SmartDashboard.h>
-#include <TimedRobot.h>
 
-#include "Commands/ExampleCommand.h"
-#include "Commands/MyAutoCommand.h"
+ExampleSubsystem Robot::m_subsystem;
+OI Robot::m_oi;
 
-class Robot : public frc::TimedRobot {
-public:
-	void RobotInit() override {
-		m_chooser.AddDefault("Default Auto", &m_defaultAuto);
-		m_chooser.AddObject("My Auto", &m_myAuto);
-		frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+void Robot::RobotInit() {
+	m_chooser.AddDefault("Default Auto", &m_defaultAuto);
+	m_chooser.AddObject("My Auto", &m_myAuto);
+	frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+}
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {
+	frc::Scheduler::GetInstance()->Run();
+}
+
+/**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable chooser
+ * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+ * remove all of the chooser code and uncomment the GetString code to get the
+ * auto name from the text box below the Gyro.
+ *
+ * You can add additional auto modes by adding additional commands to the
+ * chooser code above (like the commented example) or additional comparisons to
+ * the if-else structure below with additional strings & commands.
+ */
+void Robot::AutonomousInit() {
+	// std::string autoSelected = frc::SmartDashboard::GetString(
+	// 		"Auto Selector", "Default");
+	// if (autoSelected == "My Auto") {
+	// 	m_autonomousCommand = &m_myAuto;
+	// } else {
+	// 	m_autonomousCommand = &m_defaultAuto;
+	// }
+
+	m_autonomousCommand = m_chooser.GetSelected();
+
+	if (m_autonomousCommand != nullptr) {
+		m_autonomousCommand->Start();
 	}
+}
 
-	/**
-	 * This function is called once each time the robot enters Disabled
-	 * mode.
-	 * You can use it to reset any subsystem information you want to clear
-	 * when
-	 * the robot is disabled.
-	 */
-	void DisabledInit() override {}
+void Robot::AutonomousPeriodic() {
+	frc::Scheduler::GetInstance()->Run();
+}
 
-	void DisabledPeriodic() override {
-		frc::Scheduler::GetInstance()->Run();
+void Robot::TeleopInit() {
+	// This makes sure that the autonomous stops running when
+	// teleop starts running. If you want the autonomous to
+	// continue until interrupted by another command, remove
+	// this line or comment it out.
+	if (m_autonomousCommand != nullptr) {
+		m_autonomousCommand->Cancel();
+		m_autonomousCommand = nullptr;
 	}
+}
 
-	/**
-	 * This autonomous (along with the chooser code above) shows how to
-	 * select
-	 * between different autonomous modes using the dashboard. The sendable
-	 * chooser code works with the Java SmartDashboard. If you prefer the
-	 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
-	 * GetString code to get the auto name from the text box below the Gyro.
-	 *
-	 * You can add additional auto modes by adding additional commands to
-	 * the
-	 * chooser code above (like the commented example) or additional
-	 * comparisons
-	 * to the if-else structure below with additional strings & commands.
-	 */
-	void AutonomousInit() override {
-		std::string autoSelected = frc::SmartDashboard::GetString(
-				"Auto Selector", "Default");
-		if (autoSelected == "My Auto") {
-			m_autonomousCommand = &m_myAuto;
-		} else {
-			m_autonomousCommand = &m_defaultAuto;
-		}
+void Robot::TeleopPeriodic() {
+	frc::Scheduler::GetInstance()->Run();
+}
 
-		m_autonomousCommand = m_chooser.GetSelected();
-
-		if (m_autonomousCommand != nullptr) {
-			m_autonomousCommand->Start();
-		}
-	}
-
-	void AutonomousPeriodic() override {
-		frc::Scheduler::GetInstance()->Run();
-	}
-
-	void TeleopInit() override {
-		// This makes sure that the autonomous stops running when
-		// teleop starts running. If you want the autonomous to
-		// continue until interrupted by another command, remove
-		// this line or comment it out.
-		if (m_autonomousCommand != nullptr) {
-			m_autonomousCommand->Cancel();
-			m_autonomousCommand = nullptr;
-		}
-	}
-
-	void TeleopPeriodic() override { frc::Scheduler::GetInstance()->Run(); }
-
-	void TestPeriodic() override {}
-
-private:
-	// Have it null by default so that if testing teleop it
-	// doesn't have undefined behavior and potentially crash.
-	frc::Command* m_autonomousCommand = nullptr;
-	ExampleCommand m_defaultAuto;
-	MyAutoCommand m_myAuto;
-	frc::SendableChooser<frc::Command*> m_chooser;
-};
+void Robot::TestPeriodic() {}
 
 START_ROBOT_CLASS(Robot)

wpilibcExamples/src/main/cpp/templates/commandbased/Robot.h

@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <Commands/Command.h>
+#include <SmartDashboard/SendableChooser.h>
+#include <TimedRobot.h>
+
+#include "Commands/ExampleCommand.h"
+#include "Commands/MyAutoCommand.h"
+#include "OI.h"
+#include "Subsystems/ExampleSubsystem.h"
+
+class Robot : public frc::TimedRobot {
+public:
+	static ExampleSubsystem m_subsystem;
+	static OI m_oi;
+
+	void RobotInit() override;
+	void DisabledInit() override;
+	void DisabledPeriodic() override;
+	void AutonomousInit() override;
+	void AutonomousPeriodic() override;
+	void TeleopInit() override;
+	void TeleopPeriodic() override;
+	void TestPeriodic() override;
+
+private:
+	// Have it null by default so that if testing teleop it
+	// doesn't have undefined behavior and potentially crash.
+	frc::Command* m_autonomousCommand = nullptr;
+	ExampleCommand m_defaultAuto;
+	MyAutoCommand m_myAuto;
+	frc::SendableChooser<frc::Command*> m_chooser;
+};

wpilibcExamples/src/main/cpp/templates/iterative/Robot.cpp

@@ -5,69 +5,54 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <iostream>
-#include <string>
+#include "Robot.h"
+
+#include <iostream>
 
-#include <IterativeRobot.h>
-#include <LiveWindow/LiveWindow.h>
-#include <SmartDashboard/SendableChooser.h>
 #include <SmartDashboard/SmartDashboard.h>
 
-class Robot : public frc::IterativeRobot {
-public:
-	void RobotInit() {
-		m_chooser.AddDefault(kAutoNameDefault, kAutoNameDefault);
-		m_chooser.AddObject(kAutoNameCustom, kAutoNameCustom);
-		frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+void Robot::RobotInit() {
+	m_chooser.AddDefault(kAutoNameDefault, kAutoNameDefault);
+	m_chooser.AddObject(kAutoNameCustom, kAutoNameCustom);
+	frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+}
+
+/**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable chooser
+ * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+ * remove all of the chooser code and uncomment the GetString line to get the
+ * auto name from the text box below the Gyro.
+ *
+ * You can add additional auto modes by adding additional comparisons to the
+ * if-else structure below with additional strings. If using the SendableChooser
+ * make sure to add them to the chooser code above as well.
+ */
+void Robot::AutonomousInit() {
+	m_autoSelected = m_chooser.GetSelected();
+	// m_autoSelected = SmartDashboard::GetString(
+	// 		"Auto Selector", kAutoNameDefault);
+	std::cout << "Auto selected: " << m_autoSelected << std::endl;
+
+	if (m_autoSelected == kAutoNameCustom) {
+		// Custom Auto goes here
+	} else {
+		// Default Auto goes here
 	}
+}
 
-	/*
-	 * This autonomous (along with the chooser code above) shows how to
-	 * select
-	 * between different autonomous modes using the dashboard. The sendable
-	 * chooser code works with the Java SmartDashboard. If you prefer the
-	 * LabVIEW Dashboard, remove all of the chooser code and uncomment the
-	 * GetString line to get the auto name from the text box below the Gyro.
-	 *
-	 * You can add additional auto modes by adding additional comparisons to
-	 * the
-	 * if-else structure below with additional strings. If using the
-	 * SendableChooser make sure to add them to the chooser code above as
-	 * well.
-	 */
-	void AutonomousInit() override {
-		m_autoSelected = m_chooser.GetSelected();
-		// m_autoSelected = SmartDashboard::GetString(
-		// 		"Auto Selector", kAutoNameDefault);
-		std::cout << "Auto selected: " << m_autoSelected << std::endl;
-
-		if (m_autoSelected == kAutoNameCustom) {
-			// Custom Auto goes here
-		} else {
-			// Default Auto goes here
-		}
+void Robot::AutonomousPeriodic() {
+	if (m_autoSelected == kAutoNameCustom) {
+		// Custom Auto goes here
+	} else {
+		// Default Auto goes here
 	}
+}
 
-	void AutonomousPeriodic() {
-		if (m_autoSelected == kAutoNameCustom) {
-			// Custom Auto goes here
-		} else {
-			// Default Auto goes here
-		}
-	}
+void Robot::TeleopInit() {}
 
-	void TeleopInit() {}
+void Robot::TeleopPeriodic() {}
 
-	void TeleopPeriodic() {}
-
-	void TestPeriodic() {}
-
-private:
-	frc::LiveWindow& m_lw = *LiveWindow::GetInstance();
-	frc::SendableChooser<std::string> m_chooser;
-	const std::string kAutoNameDefault = "Default";
-	const std::string kAutoNameCustom = "My Auto";
-	std::string m_autoSelected;
-};
+void Robot::TestPeriodic() {}
 
 START_ROBOT_CLASS(Robot)

wpilibcExamples/src/main/cpp/templates/iterative/Robot.h

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <IterativeRobot.h>
+#include <SmartDashboard/SendableChooser.h>
+
+class Robot : public frc::IterativeRobot {
+public:
+	void RobotInit() override;
+	void AutonomousInit() override;
+	void AutonomousPeriodic() override;
+	void TeleopInit() override;
+	void TeleopPeriodic() override;
+	void TestPeriodic() override;
+
+private:
+	frc::SendableChooser<std::string> m_chooser;
+	const std::string kAutoNameDefault = "Default";
+	const std::string kAutoNameCustom = "My Auto";
+	std::string m_autoSelected;
+};

wpilibcExamples/src/main/cpp/templates/sample/Robot.cpp

@@ -5,121 +5,87 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <iostream>
-#include <string>
+#include "Robot.h"
+
+#include <iostream>
 
-#include <Drive/DifferentialDrive.h>
-#include <Joystick.h>
-#include <SampleRobot.h>
-#include <SmartDashboard/SendableChooser.h>
 #include <SmartDashboard/SmartDashboard.h>
-#include <Spark.h>
 #include <Timer.h>
 
+Robot::Robot() {
+	// Note SmartDashboard is not initialized here, wait until
+	// RobotInit to make SmartDashboard calls
+	m_robotDrive.SetExpiration(0.1);
+}
+
+void Robot::RobotInit() {
+	m_chooser.AddDefault(kAutoNameDefault, kAutoNameDefault);
+	m_chooser.AddObject(kAutoNameCustom, kAutoNameCustom);
+	frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+}
+
 /**
- * This is a demo program showing the use of the DifferentialDrive class.
- * The SampleRobot class is the base of a robot application that will
- * automatically call your Autonomous and OperatorControl methods at the right
- * time as controlled by the switches on the driver station or the field
- * controls.
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable chooser
+ * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+ * remove all of the chooser code and uncomment the GetString line to get the
+ * auto name from the text box below the Gyro.
  *
- * WARNING: While it may look like a good choice to use for your code if you're
- * inexperienced, don't. Unless you know what you are doing, complex code will
- * be much more difficult under this system. Use IterativeRobot or Command-Based
- * instead if you're new.
+ * You can add additional auto modes by adding additional comparisons to the
+ * if-else structure below with additional strings. If using the SendableChooser
+ * make sure to add them to the chooser code above as well.
  */
-class Robot : public frc::SampleRobot {
-public:
-	Robot() {
-		// Note SmartDashboard is not initialized here, wait until
-		// RobotInit to make SmartDashboard calls
-		m_robotDrive.SetExpiration(0.1);
+void Robot::Autonomous() {
+	std::string autoSelected = m_chooser.GetSelected();
+	// std::string autoSelected = frc::SmartDashboard::GetString(
+	// "Auto Selector", kAutoNameDefault);
+	std::cout << "Auto selected: " << autoSelected << std::endl;
+
+	// MotorSafety improves safety when motors are updated in loops
+	// but is disabled here because motor updates are not looped in
+	// this autonomous mode.
+	m_robotDrive.SetSafetyEnabled(false);
+
+	if (autoSelected == kAutoNameCustom) {
+		// Custom Auto goes here
+		std::cout << "Running custom Autonomous" << std::endl;
+
+		// Spin at half speed for two seconds
+		m_robotDrive.ArcadeDrive(0.0, 0.5);
+		frc::Wait(2.0);
+
+		// Stop robot
+		m_robotDrive.ArcadeDrive(0.0, 0.0);
+	} else {
+		// Default Auto goes here
+		std::cout << "Running default Autonomous" << std::endl;
+
+		// Drive forwards at half speed for two seconds
+		m_robotDrive.ArcadeDrive(-0.5, 0.0);
+		frc::Wait(2.0);
+
+		// Stop robot
+		m_robotDrive.ArcadeDrive(0.0, 0.0);
 	}
+}
 
-	void RobotInit() {
-		m_chooser.AddDefault(kAutoNameDefault, kAutoNameDefault);
-		m_chooser.AddObject(kAutoNameCustom, kAutoNameCustom);
-		frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+/**
+ * Runs the motors with arcade steering.
+ */
+void Robot::OperatorControl() {
+	m_robotDrive.SetSafetyEnabled(true);
+	while (IsOperatorControl() && IsEnabled()) {
+		// Drive with arcade style (use right stick)
+		m_robotDrive.ArcadeDrive(-m_stick.GetY(), m_stick.GetX());
+
+		// The motors will be updated every 5ms
+		frc::Wait(0.005);
 	}
+}
 
-	/*
-	 * This autonomous (along with the chooser code above) shows how to
-	 * select between different autonomous modes using the dashboard. The
-	 * sendable chooser code works with the Java SmartDashboard. If you
-	 * prefer the LabVIEW Dashboard, remove all of the chooser code and
-	 * uncomment the GetString line to get the auto name from the text box
-	 * below the Gyro.
-	 *
-	 * You can add additional auto modes by adding additional comparisons to
-	 * the if-else structure below with additional strings. If using the
-	 * SendableChooser make sure to add them to the chooser code above as
-	 * well.
-	 */
-	void Autonomous() {
-		std::string autoSelected = m_chooser.GetSelected();
-		// std::string autoSelected = frc::SmartDashboard::GetString(
-		// "Auto Selector", kAutoNameDefault);
-		std::cout << "Auto selected: " << autoSelected << std::endl;
-
-		// MotorSafety improves safety when motors are updated in loops
-		// but is disabled here because motor updates are not looped in
-		// this autonomous mode.
-		m_robotDrive.SetSafetyEnabled(false);
-
-		if (autoSelected == kAutoNameCustom) {
-			// Custom Auto goes here
-			std::cout << "Running custom Autonomous" << std::endl;
-
-			// Spin at half speed for two seconds
-			m_robotDrive.ArcadeDrive(0.0, 0.5);
-			frc::Wait(2.0);
-
-			// Stop robot
-			m_robotDrive.ArcadeDrive(0.0, 0.0);
-		} else {
-			// Default Auto goes here
-			std::cout << "Running default Autonomous" << std::endl;
-
-			// Drive forwards at half speed for two seconds
-			m_robotDrive.ArcadeDrive(-0.5, 0.0);
-			frc::Wait(2.0);
-
-			// Stop robot
-			m_robotDrive.ArcadeDrive(0.0, 0.0);
-		}
-	}
-
-	/*
-	 * Runs the motors with arcade steering.
-	 */
-	void OperatorControl() override {
-		m_robotDrive.SetSafetyEnabled(true);
-		while (IsOperatorControl() && IsEnabled()) {
-			// Drive with arcade style (use right stick)
-			m_robotDrive.ArcadeDrive(
-					-m_stick.GetY(), m_stick.GetX());
-
-			// The motors will be updated every 5ms
-			frc::Wait(0.005);
-		}
-	}
-
-	/*
-	 * Runs during test mode
-	 */
-	void Test() override {}
-
-private:
-	// Robot drive system
-	frc::Spark m_leftMotor{0};
-	frc::Spark m_rightMotor{1};
-	frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
-
-	frc::Joystick m_stick{0};
-
-	frc::SendableChooser<std::string> m_chooser;
-	const std::string kAutoNameDefault = "Default";
-	const std::string kAutoNameCustom = "My Auto";
-};
+/**
+ * Runs during test mode
+ */
+void Robot::Test() {}
 
 START_ROBOT_CLASS(Robot)

wpilibcExamples/src/main/cpp/templates/sample/Robot.h

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <Drive/DifferentialDrive.h>
+#include <Joystick.h>
+#include <SampleRobot.h>
+#include <SmartDashboard/SendableChooser.h>
+#include <Spark.h>
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * The SampleRobot class is the base of a robot application that will
+ * automatically call your Autonomous and OperatorControl methods at the right
+ * time as controlled by the switches on the driver station or the field
+ * controls.
+ *
+ * WARNING: While it may look like a good choice to use for your code if you're
+ * inexperienced, don't. Unless you know what you are doing, complex code will
+ * be much more difficult under this system. Use IterativeRobot or Command-Based
+ * instead if you're new.
+ */
+class Robot : public frc::SampleRobot {
+public:
+	Robot();
+
+	void RobotInit() override;
+	void Autonomous() override;
+	void OperatorControl() override;
+	void Test() override;
+
+private:
+	// Robot drive system
+	frc::Spark m_leftMotor{0};
+	frc::Spark m_rightMotor{1};
+	frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+
+	frc::Joystick m_stick{0};
+
+	frc::SendableChooser<std::string> m_chooser;
+	const std::string kAutoNameDefault = "Default";
+	const std::string kAutoNameCustom = "My Auto";
+};

wpilibcExamples/src/main/cpp/templates/timed/Robot.cpp

@@ -5,68 +5,54 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+#include "Robot.h"
+
 #include <iostream>
-#include <string>
 
-#include <LiveWindow/LiveWindow.h>
-#include <SmartDashboard/SendableChooser.h>
 #include <SmartDashboard/SmartDashboard.h>
-#include <TimedRobot.h>
 
-class Robot : public frc::TimedRobot {
-public:
-	void RobotInit() {
-		m_chooser.AddDefault(kAutoNameDefault, kAutoNameDefault);
-		m_chooser.AddObject(kAutoNameCustom, kAutoNameCustom);
-		frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+void Robot::RobotInit() {
+	m_chooser.AddDefault(kAutoNameDefault, kAutoNameDefault);
+	m_chooser.AddObject(kAutoNameCustom, kAutoNameCustom);
+	frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
+}
+
+/**
+ * This autonomous (along with the chooser code above) shows how to select
+ * between different autonomous modes using the dashboard. The sendable chooser
+ * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
+ * remove all of the chooser code and uncomment the GetString line to get the
+ * auto name from the text box below the Gyro.
+ *
+ * You can add additional auto modes by adding additional comparisons to the
+ * if-else structure below with additional strings. If using the SendableChooser
+ * make sure to add them to the chooser code above as well.
+ */
+void Robot::AutonomousInit() {
+	m_autoSelected = m_chooser.GetSelected();
+	// m_autoSelected = SmartDashboard::GetString("Auto Selector",
+	//		 kAutoNameDefault);
+	std::cout << "Auto selected: " << m_autoSelected << std::endl;
+
+	if (m_autoSelected == kAutoNameCustom) {
+		// Custom Auto goes here
+	} else {
+		// Default Auto goes here
 	}
+}
 
-	/*
-	 * This autonomous (along with the chooser code above) shows how to
-	 * select between different autonomous modes using the dashboard. The
-	 * sendable chooser code works with the Java SmartDashboard. If you
-	 * prefer the LabVIEW Dashboard, remove all of the chooser code and
-	 * uncomment the GetString line to get the auto name from the text box
-	 * below the Gyro.
-	 *
-	 * You can add additional auto modes by adding additional comparisons to
-	 * the if-else structure below with additional strings. If using the
-	 * SendableChooser make sure to add them to the chooser code above as
-	 * well.
-	 */
-	void AutonomousInit() override {
-		m_autoSelected = m_chooser.GetSelected();
-		// m_autoSelected = SmartDashboard::GetString("Auto Selector",
-		//		 kAutoNameDefault);
-		std::cout << "Auto selected: " << m_autoSelected << std::endl;
-
-		if (m_autoSelected == kAutoNameCustom) {
-			// Custom Auto goes here
-		} else {
-			// Default Auto goes here
-		}
+void Robot::AutonomousPeriodic() {
+	if (m_autoSelected == kAutoNameCustom) {
+		// Custom Auto goes here
+	} else {
+		// Default Auto goes here
 	}
+}
 
-	void AutonomousPeriodic() {
-		if (m_autoSelected == kAutoNameCustom) {
-			// Custom Auto goes here
-		} else {
-			// Default Auto goes here
-		}
-	}
+void Robot::TeleopInit() {}
 
-	void TeleopInit() {}
+void Robot::TeleopPeriodic() {}
 
-	void TeleopPeriodic() {}
-
-	void TestPeriodic() {}
-
-private:
-	frc::LiveWindow& m_lw = *LiveWindow::GetInstance();
-	frc::SendableChooser<std::string> m_chooser;
-	const std::string kAutoNameDefault = "Default";
-	const std::string kAutoNameCustom = "My Auto";
-	std::string m_autoSelected;
-};
+void Robot::TestPeriodic() {}
 
 START_ROBOT_CLASS(Robot)

wpilibcExamples/src/main/cpp/templates/timed/Robot.h

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <SmartDashboard/SendableChooser.h>
+#include <TimedRobot.h>
+
+class Robot : public frc::TimedRobot {
+public:
+	void RobotInit() override;
+	void AutonomousInit() override;
+	void AutonomousPeriodic() override;
+	void TeleopInit() override;
+	void TeleopPeriodic() override;
+	void TestPeriodic() override;
+
+private:
+	frc::SendableChooser<std::string> m_chooser;
+	const std::string kAutoNameDefault = "Default";
+	const std::string kAutoNameCustom = "My Auto";
+	std::string m_autoSelected;
+};

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

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

@@ -80,6 +80,10 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
     setName("ADXRS450_Gyro", port.value);
   }
 
+  public boolean isConnected() {
+    return m_spi != null;
+  }
+
   @Override
   public void calibrate() {
     if (m_spi == null) {
@@ -128,7 +132,9 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
 
   @Override
   public void reset() {
-    m_spi.resetAccumulator();
+    if (m_spi != null) {
+      m_spi.resetAccumulator();
+    }
   }
 
   /**

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

@@ -252,39 +252,43 @@ public class CameraServer {
     }
 
     NetworkTableEntry entry = table.getEntry(name);
-    switch (event.propertyKind) {
-      case kBoolean:
-        if (isNew) {
-          entry.setDefaultBoolean(event.value != 0);
-        } else {
-          entry.setBoolean(event.value != 0);
-        }
-        break;
-      case kInteger:
-      case kEnum:
-        if (isNew) {
-          entry.setDefaultDouble(event.value);
-          table.getEntry(infoName + "/min").setDouble(
-              CameraServerJNI.getPropertyMin(event.propertyHandle));
-          table.getEntry(infoName + "/max").setDouble(
-              CameraServerJNI.getPropertyMax(event.propertyHandle));
-          table.getEntry(infoName + "/step").setDouble(
-              CameraServerJNI.getPropertyStep(event.propertyHandle));
-          table.getEntry(infoName + "/default").setDouble(
-              CameraServerJNI.getPropertyDefault(event.propertyHandle));
-        } else {
-          entry.setDouble(event.value);
-        }
-        break;
-      case kString:
-        if (isNew) {
-          entry.setDefaultString(event.valueStr);
-        } else {
-          entry.setString(event.valueStr);
-        }
-        break;
-      default:
-        break;
+    try {
+      switch (event.propertyKind) {
+        case kBoolean:
+          if (isNew) {
+            entry.setDefaultBoolean(event.value != 0);
+          } else {
+            entry.setBoolean(event.value != 0);
+          }
+          break;
+        case kInteger:
+        case kEnum:
+          if (isNew) {
+            entry.setDefaultDouble(event.value);
+            table.getEntry(infoName + "/min").setDouble(
+                CameraServerJNI.getPropertyMin(event.propertyHandle));
+            table.getEntry(infoName + "/max").setDouble(
+                CameraServerJNI.getPropertyMax(event.propertyHandle));
+            table.getEntry(infoName + "/step").setDouble(
+                CameraServerJNI.getPropertyStep(event.propertyHandle));
+            table.getEntry(infoName + "/default").setDouble(
+                CameraServerJNI.getPropertyDefault(event.propertyHandle));
+          } else {
+            entry.setDouble(event.value);
+          }
+          break;
+        case kString:
+          if (isNew) {
+            entry.setDefaultString(event.valueStr);
+          } else {
+            entry.setString(event.valueStr);
+          }
+          break;
+        default:
+          break;
+      }
+    } catch (VideoException ex) {
+      // ignore
     }
   }
 
@@ -388,8 +392,12 @@ public class CameraServer {
         case kSourcePropertyChoicesUpdated: {
           NetworkTable table = m_tables.get(event.sourceHandle);
           if (table != null) {
-            String[] choices = CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
-            table.getEntry("PropertyInfo/" + event.name + "/choices").setStringArray(choices);
+            try {
+              String[] choices = CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
+              table.getEntry("PropertyInfo/" + event.name + "/choices").setStringArray(choices);
+            } catch (VideoException ex) {
+              // ignore
+            }
           }
           break;
         }

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/Notifier.java

@@ -99,6 +99,18 @@ public class Notifier {
       }
     });
     m_thread.setDaemon(true);
+    m_thread.setUncaughtExceptionHandler((thread, error) -> {
+      Throwable cause = error.getCause();
+      if (cause != null) {
+        error = cause;
+      }
+      DriverStation.reportError("Unhandled exception: " + error.toString(), error.getStackTrace());
+      DriverStation.reportWarning("Robots should not quit, but yours did!", false);
+      DriverStation.reportError(
+          "The loopFunc() method (or methods called by it) should have handled "
+              + "the exception above.", false);
+      System.exit(1);
+    });
     m_thread.start();
   }
 

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
-   * shortest route to the setpoint.
+   * min input range as constraints, it considers them to be the same point and automatically
+   * 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
-   * shortest route to the setpoint.
+   * min input range as constraints, it considers them to be the same point and automatically
+   * 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/PWM.java

@@ -244,8 +244,8 @@ public class PWM extends SendableBase implements Sendable {
 
   @Override
   public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("Speed Controller");
+    builder.setSmartDashboardType("PWM");
     builder.setSafeState(this::setDisabled);
-    builder.addDoubleProperty("Value", this::getSpeed, this::setSpeed);
+    builder.addDoubleProperty("Value", this::getRaw, value -> setRaw((int) value));
   }
 }

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

@@ -7,6 +7,8 @@
 
 package edu.wpi.first.wpilibj;
 
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+
 /**
  * Common base class for all PWM Speed Controllers.
  */
@@ -66,4 +68,11 @@ public abstract class PWMSpeedController extends SafePWM implements SpeedControl
   public void pidWrite(double output) {
     set(output);
   }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Speed Controller");
+    builder.setSafeState(this::setDisabled);
+    builder.addDoubleProperty("Value", this::getSpeed, this::setSpeed);
+  }
 }

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/SerialPort.java

@@ -94,6 +94,42 @@ public class SerialPort {
     }
   }
 
+  /**
+   * Create an instance of a Serial Port class.
+   *
+   * @param baudRate The baud rate to configure the serial port.
+   * @param port     The Serial port to use
+   * @param portName The direct portName to use
+   * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
+   * @param parity   Select the type of parity checking to use.
+   * @param stopBits The number of stop bits to use as defined by the enum StopBits.
+   * @deprecated     Will be removed for 2019
+   */
+  @Deprecated
+  public SerialPort(final int baudRate, String portName, Port port, final int dataBits,
+                    Parity parity, StopBits stopBits) {
+    m_port = (byte) port.value;
+
+    SerialPortJNI.serialInitializePortDirect(m_port, portName);
+    SerialPortJNI.serialSetBaudRate(m_port, baudRate);
+    SerialPortJNI.serialSetDataBits(m_port, (byte) dataBits);
+    SerialPortJNI.serialSetParity(m_port, (byte) parity.value);
+    SerialPortJNI.serialSetStopBits(m_port, (byte) stopBits.value);
+
+    // Set the default read buffer size to 1 to return bytes immediately
+    setReadBufferSize(1);
+
+    // Set the default timeout to 5 seconds.
+    setTimeout(5.0);
+
+    // Don't wait until the buffer is full to transmit.
+    setWriteBufferMode(WriteBufferMode.kFlushOnAccess);
+
+    disableTermination();
+
+    HAL.report(tResourceType.kResourceType_SerialPort, 0);
+  }
+
   /**
    * Create an instance of a Serial Port class.
    *

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) {
-      speedController.pidWrite(output);
-    }
+    set(output);
   }
 
   @Override

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

@@ -21,7 +21,7 @@ import edu.wpi.first.wpilibj.hal.HAL;
 public class TimedRobot extends IterativeRobotBase {
   public static final double DEFAULT_PERIOD = 0.02;
 
-  private double m_period = DEFAULT_PERIOD;
+  private volatile double m_period = DEFAULT_PERIOD;
 
   // Prevents loop from starting if user calls setPeriod() in robotInit()
   private boolean m_startLoop = false;
@@ -68,4 +68,11 @@ public class TimedRobot extends IterativeRobotBase {
       m_loop.startPeriodic(m_period);
     }
   }
+
+  /**
+   * Get time period between calls to Periodic() functions.
+   */
+  public double getPeriod() {
+    return m_period;
+  }
 }

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()
-        && m_chosenCommand.isFinished();
+    if (m_chosenCommand != null) {
+      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/drive/DifferentialDrive.java

@@ -261,6 +261,13 @@ public class DifferentialDrive extends RobotDriveBase {
       }
     }
 
+    // Normalize the wheel speeds
+    double maxMagnitude = Math.max(Math.abs(leftMotorOutput), Math.abs(rightMotorOutput));
+    if (maxMagnitude > 1.0) {
+      leftMotorOutput /= maxMagnitude;
+      rightMotorOutput /= maxMagnitude;
+    }
+
     m_leftMotor.set(leftMotorOutput * m_maxOutput);
     m_rightMotor.set(-rightMotorOutput * m_maxOutput);
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java

@@ -189,11 +189,11 @@ public class MecanumDrive extends RobotDriveBase {
     builder.setSmartDashboardType("MecanumDrive");
     builder.addDoubleProperty("Front Left Motor Speed", m_frontLeftMotor::get,
         m_frontLeftMotor::set);
-    builder.addDoubleProperty("Front Right Motor Speed", m_frontRightMotor::get,
-        m_frontRightMotor::set);
+    builder.addDoubleProperty("Front Right Motor Speed", () -> -m_frontRightMotor.get(),
+        value -> m_frontRightMotor.set(-value));
     builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get,
         m_rearLeftMotor::set);
-    builder.addDoubleProperty("Rear Right Motor Speed", m_rearRightMotor::get,
-        m_rearRightMotor::set);
+    builder.addDoubleProperty("Rear Right Motor Speed", () -> -m_rearRightMotor.get(),
+        value -> m_rearRightMotor.set(-value));
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java

@@ -43,9 +43,11 @@ public abstract class RobotDriveBase extends SendableBase implements MotorSafety
   }
 
   /**
-   * Change the default value for deadband scaling. The default value is
-   * {@value #kDefaultDeadband}. Values smaller then the deadband are set to 0, while values
-   * larger then the deadband are scaled from 0.0 to 1.0. See {@link #applyDeadband}.
+   * Sets the deadband applied to the drive inputs (e.g., joystick values).
+   *
+   * <p>The default value is {@value #kDefaultDeadband}. Inputs smaller than the deadband are set to
+   * 0.0 while inputs larger than the deadband are scaled from 0.0 to 1.0. See
+   * {@link #applyDeadband}.
    *
    * @param deadband The deadband to set.
    */

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

@@ -15,8 +15,11 @@ public class DIOJNI extends JNIWrapper {
 
   public static native void freeDIOPort(int dioPortHandle);
 
+  // TODO(Thad): Switch this to use boolean
   public static native void setDIO(int dioPortHandle, short value);
 
+  public static native void setDIODirection(int dioPortHandle, boolean input);
+
   public static native boolean getDIO(int dioPortHandle);
 
   public static native boolean getDIODirection(int dioPortHandle);

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/main/java/edu/wpi/first/wpilibj/hal/SerialPortJNI.java

@@ -10,6 +10,8 @@ package edu.wpi.first.wpilibj.hal;
 public class SerialPortJNI extends JNIWrapper {
   public static native void serialInitializePort(byte port);
 
+  public static native void serialInitializePortDirect(byte port, String portName);
+
   public static native void serialSetBaudRate(byte port, int baud);
 
   public static native void serialSetDataBits(byte port, byte bits);

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java

@@ -51,6 +51,7 @@ public class SendableChooser<V> extends SendableBase implements Sendable {
    * Instantiates a {@link SendableChooser}.
    */
   public SendableChooser() {
+    super(false);
   }
 
   /**

wpilibj/src/main/native/cpp/DIOJNI.cpp

@@ -90,6 +90,22 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_setDIO(
   CheckStatus(env, status);
 }
 
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_DIOJNI
+ * Method:    setDIODirection
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_DIOJNI_setDIODirection(
+  JNIEnv *env, jclass, jint id, jboolean input) {
+  // DIOJNI_LOG(logDEBUG) << "Calling DIOJNI setDIO";
+  // DIOJNI_LOG(logDEBUG) << "Port Handle = " << (HAL_DigitalHandle)id;
+  // DIOJNI_LOG(logDEBUG) << "IsInput = " << input;
+  int32_t status = 0;
+  HAL_SetDIODirection((HAL_DigitalHandle)id, input, &status);
+  // DIOJNI_LOG(logDEBUG) << "Status = " << status;
+  CheckStatus(env, status);
+}
+
 /*
  * Class:     edu_wpi_first_wpilibj_hal_DIOJNI
  * Method:    getDIO

wpilibj/src/main/native/cpp/EncoderJNI.cpp

@@ -36,7 +36,7 @@ extern "C" {
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_wpilibj_hal_EncoderJNI_initializeEncoder(
     JNIEnv* env, jclass, jint digitalSourceHandleA, jint analogTriggerTypeA,
-    jint digitalSourceHandleB, jint analogTriggerTypeB, jboolean reverseDirection, 
+    jint digitalSourceHandleB, jint analogTriggerTypeB, jboolean reverseDirection,
     jint encodingType) {
   ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI initializeEncoder";
   ENCODERJNI_LOG(logDEBUG) << "Source Handle A = " << digitalSourceHandleA;
@@ -49,10 +49,10 @@ Java_edu_wpi_first_wpilibj_hal_EncoderJNI_initializeEncoder(
   ENCODERJNI_LOG(logDEBUG) << "EncodingType = " << encodingType;
   int32_t status = 0;
   auto encoder = HAL_InitializeEncoder(
-      (HAL_Handle)digitalSourceHandleA, (HAL_AnalogTriggerType)analogTriggerTypeA, 
+      (HAL_Handle)digitalSourceHandleA, (HAL_AnalogTriggerType)analogTriggerTypeA,
       (HAL_Handle)digitalSourceHandleB, (HAL_AnalogTriggerType)analogTriggerTypeB,
       reverseDirection, (HAL_EncoderEncodingType)encodingType, &status);
-      
+
   ENCODERJNI_LOG(logDEBUG) << "Status = " << status;
   ENCODERJNI_LOG(logDEBUG) << "ENCODER Handle = " << encoder;
   CheckStatusForceThrow(env, status);
@@ -340,7 +340,7 @@ Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderSamplesToAverage(
  */
 JNIEXPORT void JNICALL
 Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderIndexSource(
-    JNIEnv* env, jclass, jint id, jint digitalSourceHandle, 
+    JNIEnv* env, jclass, jint id, jint digitalSourceHandle,
     jint analogTriggerType, jint type) {
   ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI setEncoderIndexSource";
   ENCODERJNI_LOG(logDEBUG) << "Encoder Handle = " << (HAL_EncoderHandle)id;
@@ -349,8 +349,8 @@ Java_edu_wpi_first_wpilibj_hal_EncoderJNI_setEncoderIndexSource(
                            << analogTriggerType;
   ENCODERJNI_LOG(logDEBUG) << "IndexingType = " << type;
   int32_t status = 0;
-  HAL_SetEncoderIndexSource((HAL_EncoderHandle)id, (HAL_Handle)digitalSourceHandle, 
-                            (HAL_AnalogTriggerType)analogTriggerType, 
+  HAL_SetEncoderIndexSource((HAL_EncoderHandle)id, (HAL_Handle)digitalSourceHandle,
+                            (HAL_AnalogTriggerType)analogTriggerType,
                             (HAL_EncoderIndexingType)type, &status);
   ENCODERJNI_LOG(logDEBUG) << "Status = " << status;
   CheckStatus(env, status);
@@ -405,7 +405,7 @@ Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderDecodingScaleFactor(
   ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderSamplesToAverage";
   ENCODERJNI_LOG(logDEBUG) << "Encoder Handle = " << (HAL_EncoderHandle)id;
   int32_t status = 0;
-  jint returnValue = HAL_GetEncoderDecodingScaleFactor((HAL_EncoderHandle)id, &status);
+  jdouble returnValue = HAL_GetEncoderDecodingScaleFactor((HAL_EncoderHandle)id, &status);
   ENCODERJNI_LOG(logDEBUG) << "Status = " << status;
   ENCODERJNI_LOG(logDEBUG) << "getEncoderSamplesToAverageResult = "
                            << returnValue;
@@ -424,7 +424,7 @@ Java_edu_wpi_first_wpilibj_hal_EncoderJNI_getEncoderDistancePerPulse(
   ENCODERJNI_LOG(logDEBUG) << "Calling ENCODERJNI getEncoderSamplesToAverage";
   ENCODERJNI_LOG(logDEBUG) << "Encoder Handle = " << (HAL_EncoderHandle)id;
   int32_t status = 0;
-  jint returnValue = HAL_GetEncoderDistancePerPulse((HAL_EncoderHandle)id, &status);
+  jdouble returnValue = HAL_GetEncoderDistancePerPulse((HAL_EncoderHandle)id, &status);
   ENCODERJNI_LOG(logDEBUG) << "Status = " << status;
   ENCODERJNI_LOG(logDEBUG) << "getEncoderSamplesToAverageResult = "
                            << returnValue;

wpilibj/src/main/native/cpp/SerialPortJNI.cpp

@@ -45,6 +45,25 @@ Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialInitializePort(
   CheckStatusForceThrow(env, status);
 }
 
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_SerialPortJNI
+ * Method:    serialInitializePortDirect
+ * Signature: (BLjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialInitializePortDirect(
+    JNIEnv* env, jclass, jbyte port, jstring portName) {
+  SERIALJNI_LOG(logDEBUG) << "Calling Serial Initialize Direct";
+  SERIALJNI_LOG(logDEBUG) << "Port = " << (jint)port;
+  JStringRef portNameRef{env, portName};
+  SERIALJNI_LOG(logDEBUG) << "PortName = " << portNameRef.c_str();
+  int32_t status = 0;
+  HAL_InitializeSerialPortDirect(static_cast<HAL_SerialPort>(port),
+                                 portNameRef.c_str(), &status);
+  SERIALJNI_LOG(logDEBUG) << "Status = " << status;
+  CheckStatusForceThrow(env, status);
+}
+
 /*
  * Class:     edu_wpi_first_wpilibj_hal_SerialPortJNI
  * Method:    serialSetBaudRate
@@ -247,7 +266,7 @@ JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_SerialPortJNI_serialRead(
   llvm::SmallVector<char, 128> recvBuf;
   recvBuf.resize(size);
   int32_t status = 0;
-  jint retVal = HAL_ReadSerial(static_cast<HAL_SerialPort>(port), recvBuf.data(), 
+  jint retVal = HAL_ReadSerial(static_cast<HAL_SerialPort>(port), recvBuf.data(),
                                size, &status);
   env->SetByteArrayRegion(dataReceived, 0, size,
                           reinterpret_cast<const jbyte *>(recvBuf.data()));

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_onFalse = new MockCommand();
+    m_subsys = new MockSubsystem();
+    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() {}
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java

@@ -23,8 +23,7 @@ import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem;
  * project.
  */
 public class Robot extends TimedRobot {
-	public static final ExampleSubsystem kExampleSubsystem
-			= new ExampleSubsystem();
+	public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
 	public static OI m_oi;
 
 	Command m_autonomousCommand;

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java

@@ -16,7 +16,7 @@ import edu.wpi.first.wpilibj.templates.commandbased.Robot;
 public class ExampleCommand extends Command {
 	public ExampleCommand() {
 		// Use requires() here to declare subsystem dependencies
-		requires(Robot.kExampleSubsystem);
+		requires(Robot.m_subsystem);
 	}
 
 	// Called just before this Command runs the first time