Adds TriState JNI entry point (#938 )

Also adds missing sim TriState DIO HAL call, and a ToDo for later
Invert right side motors in MecanumDrive sendable (#933 )
2026-06-20 00:51:42 +00:00 · 2018-02-12 16:05:10 -08:00 · 2018-02-09 08:30:12 -08:00 · 2018-02-04 22:38:19 -08:00 · 2018-02-01 21:39:06 -08:00 · 2018-02-01 21:17:04 -08:00
45 changed files with 1389 additions and 130 deletions
--- a/hal/src/main/native/athena/ctre/CtreCanNode.cpp
+++ b/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 */
 			}
 		}
 	}
--- a/hal/src/main/native/athena/ctre/CtreCanNode.h
+++ b/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
--- a/hal/src/main/native/include/HAL/handles/IndexedClassedHandleResource.h
+++ b/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
--- a/hal/src/main/native/sim/DIO.cpp
+++ b/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.
--- a/hal/src/test/native/cpp/handles/HandleTests.cpp
+++ b/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
--- a/ni-libraries/lib/libFRC_NetworkCommunication.so.18.0.0
+++ b/ni-libraries/lib/libFRC_NetworkCommunication.so.18.0.0
--- a/ni-libraries/lib/libRoboRIO_FRC_ChipObject.so.18.0.0
+++ b/ni-libraries/lib/libRoboRIO_FRC_ChipObject.so.18.0.0
--- a/wpilibc/src/main/native/cpp/Commands/Command.cpp
+++ b/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.
 *
--- a/wpilibc/src/main/native/cpp/Commands/ConditionalCommand.cpp
+++ b/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();
 }
--- a/wpilibc/src/main/native/cpp/Drive/DifferentialDrive.cpp
+++ b/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);

--- a/wpilibc/src/main/native/cpp/Drive/MecanumDrive.cpp
+++ b/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); });
 }
--- a/wpilibc/src/main/native/cpp/DriverStation.cpp
+++ b/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++) {
--- a/wpilibc/src/main/native/cpp/Joystick.cpp
+++ b/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]);
 }

 /**
--- a/wpilibc/src/main/native/cpp/PIDController.cpp
+++ b/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) {
--- a/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
+++ b/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");
--- a/wpilibc/src/main/native/cpp/TimedRobot.cpp
+++ b/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);

--- a/wpilibc/src/main/native/include/Commands/Command.h
+++ b/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;

--- a/wpilibc/src/main/native/include/Commands/ConditionalCommand.h
+++ b/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
--- a/wpilibc/src/main/native/include/DriverStation.h
+++ b/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
--- a/wpilibc/src/main/native/include/TimedRobot.h
+++ b/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;
--- a/wpilibcIntegrationTests/src/FRCUserProgram/cpp/command/ConditionalCommandTest.cpp
+++ b/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();
+}
--- a/wpilibcIntegrationTests/src/FRCUserProgram/cpp/command/MockCommand.cpp
+++ b/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;
+}
--- a/wpilibcIntegrationTests/src/FRCUserProgram/cpp/command/MockConditionalCommand.cpp
+++ b/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;
+}
--- a/wpilibcIntegrationTests/src/FRCUserProgram/headers/command/MockCommand.h
+++ b/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;
--- a/wpilibcIntegrationTests/src/FRCUserProgram/headers/command/MockConditionalCommand.h
+++ b/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
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
+++ b/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();
  }

  /**
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
+++ b/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();
  }

--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
+++ b/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) {
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
+++ b/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();
  }

--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
+++ b/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 {
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
+++ b/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
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
+++ b/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;
+  }
 }
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/Command.java
+++ b/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.
   *
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
+++ b/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();
  }
 }
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
+++ b/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);

--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
+++ b/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));
  }
 }
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/DIOJNI.java
+++ b/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);
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/HAL.java
+++ b/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) {
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/hal/MatchInfoData.java
+++ b/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.
--- a/wpilibj/src/main/native/cpp/DIOJNI.cpp
+++ b/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
--- a/wpilibj/src/main/native/cpp/EncoderJNI.cpp
+++ b/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;
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
+++ b/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);
+  }
 }
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
+++ b/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;
+  }
+
 }
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
+++ b/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;
+  }
 }
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
+++ b/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() {}
+}
Author	SHA1	Message	Date
Thad House	f5a292dadd	Adds TriState JNI entry point (#938 ) Also adds missing sim TriState DIO HAL call, and a ToDo for later	2018-02-12 16:05:10 -08:00
Sam Carlberg	77d6c11743	Invert right side motors in MecanumDrive sendable (#933 ) This aligns with the current behavior of DifferentialDrive Fixes shuffleboard#404	2018-02-09 08:30:12 -08:00
Tyler Veness	67f9c9a5b3	Fixed TimedRobot.java hanging if an exception was thrown (#926 )	2018-02-04 22:38:19 -08:00
Thad House	f720cbb121	Switches CtreCanNode to use locking and std::chrono for time (#909 )	2018-02-01 21:39:06 -08:00
Tyler Veness	64a7e57fe0	Added output normalization to DifferentialDrive::CurvatureDrive() (#924 ) This normalizes within -1..1 to avoid clipping and maintain the ratio between wheel speeds, since that ratio determines the center of rotation. Fixes #923.	2018-02-01 21:17:04 -08:00
Tyler Veness	5ca00dddbe	Added TimedRobot::GetPeriod() (#915 ) Fixes #914.	2018-01-27 01:01:15 -08:00
Tyler Veness	120ceb3427	Fix channel reassignments for C++ Joystick twist and throttle axes (#903 )	2018-01-26 17:26:10 -08:00
Thad House	5cbafc1382	Updates to image 17 (#913 ) These should be binary compatible, so safe to use with image 16.	2018-01-26 17:21:13 -08:00
Thad House	39d1650d51	Fixes double to int to double cast in encoderJNI (#918 ) Fixes #916	2018-01-26 17:20:20 -08:00
Thad House	02336fc478	Makes FMS data never be a null string. (#900 ) Fixes #895	2018-01-19 23:49:34 -08:00
Thad House	c00848c060	Fixes indexed classed handle resource (#899 ) Nothing in WPILib uses it, so it was not tested	2018-01-19 22:31:59 -08:00
Dustin Spicuzza	738a1c015c	PIDController: setContinuous should only check input range if continuous is true (#896 )	2018-01-19 22:31:08 -08:00
Thad House	48ae6c954a	Publishes match specific data and other FMS info to NT (#874 ) This is so products like SB can present the current setup.	2018-01-18 23:17:28 -08:00
Thad House	07f70cf784	Fixes control data packet delay (#875 ) Because of an expected change in 2018 that didn't happen, we had a race condition causing a 1 packet delay on all DS values. This fixes that.	2018-01-18 21:54:33 -08:00
sciencewhiz	e4e1eab413	Fix cancel of inner commands in ConditionalCommands (#858 )	2018-01-18 20:04:33 -08:00
Nic Hodlofski	0e8ff4663d	SpeedControllerGroup: Call set() from pidWrite() This means pidWrite() now takes m_isInverted into account. Fixes #887.	2018-01-18 20:03:13 -08:00
HeroCC	54a0a7654a	Link to replacements for RobotDrive in JavaDocs (#879 ) In smart IDEs, this will allow users to easily view source for these two classes by linking to it	2018-01-11 22:17:33 -08:00
Rohit Vighne	59f938b584	Invert when getting motor speed in SpeedControllerGroup (#886 )	2018-01-11 22:16:42 -08:00
sciencewhiz	5513888457	Fix PIDController with Continous and no Input Range set. (#883 )	2018-01-11 21:06:25 -08:00
Peter Johnson	02b6615042	RobotController: Make getBatteryVoltage() static. (#869 )	2018-01-04 23:12:13 -06:00