diff --git a/wpilibc/build.gradle b/wpilibc/build.gradle
index 5f93f7e..8730824 100644
--- a/wpilibc/build.gradle
+++ b/wpilibc/build.gradle
@@ -50,18 +50,6 @@
     }
 }
 
-ext {
-    sharedCvConfigs = [wpilibc    : [],
-                       wpilibcBase: [],
-                       wpilibcDev : [],
-                       wpilibcTest: []]
-    staticCvConfigs = [:]
-    useJava = false
-    useCpp = true
-}
-
-apply from: "${rootDir}/shared/opencv.gradle"
-
 project(':').libraryBuild.dependsOn build
 
 ext {
@@ -72,16 +60,6 @@
 
 apply from: "${rootDir}/shared/googletest.gradle"
 
-ext {
-    chipObjectComponents = ["$nativeName".toString(), "${nativeName}Dev".toString(), "${nativeName}Base".toString(),
-                            "${nativeName}Test".toString()]
-    netCommComponents = ["$nativeName".toString(), "${nativeName}Dev".toString(), "${nativeName}Base".toString(),
-                            "${nativeName}Test".toString()]
-    useNiJava = false
-}
-
-apply from: "${rootDir}/shared/nilibraries.gradle"
-
 apply plugin: DisableBuildingGTest
 
 nativeUtils.exportsConfigs {
@@ -147,6 +125,7 @@
                 } else {
                     lib project: ':cscore', library: 'cscore', linkage: 'shared'
                     lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+                    nativeUtils.useRequiredLibrary(it, 'opencv_shared')
                 }
 
             }
@@ -174,6 +153,10 @@
                 project(':hal').addHalDependency(it, 'shared')
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                 lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+                nativeUtils.useRequiredLibrary(it, 'opencv_shared')
+                if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                }
             }
         }
     }
@@ -219,7 +202,11 @@
             project(':hal').addHalDependency(it, 'shared')
             lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
             lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+            nativeUtils.useRequiredLibrary(it, 'opencv_shared')
             lib library: nativeName, linkage: 'shared'
+            if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+            }
         }
     }
     tasks {
diff --git a/wpilibc/publish.gradle b/wpilibc/publish.gradle
index dd7601e..025c09d 100644
--- a/wpilibc/publish.gradle
+++ b/wpilibc/publish.gradle
@@ -7,8 +7,8 @@
 def outputsFolder = file("$project.buildDir/outputs")
 
 task cppSourcesZip(type: Zip) {
-    destinationDir = outputsFolder
-    baseName = zipBaseName
+    destinationDirectory = outputsFolder
+    archiveBaseName = zipBaseName
     classifier = "sources"
 
     from(licenseFile) {
@@ -26,8 +26,8 @@
 cppSourcesZip.dependsOn generateCppVersion
 
 task cppHeadersZip(type: Zip) {
-    destinationDir = outputsFolder
-    baseName = zipBaseName
+    destinationDirectory = outputsFolder
+    archiveBaseName = zipBaseName
     classifier = "headers"
 
     from(licenseFile) {
diff --git a/wpilibc/src/dev/native/cpp/main.cpp b/wpilibc/src/dev/native/cpp/main.cpp
index 6e64d2b..f1370bb 100644
--- a/wpilibc/src/dev/native/cpp/main.cpp
+++ b/wpilibc/src/dev/native/cpp/main.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,7 @@
 
 #include <iostream>
 
-#include <hal/HAL.h>
+#include <hal/HALBase.h>
 
 #include "WPILibVersion.h"
 
diff --git a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
index 5b5762a..a760daa 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/ADXL345_I2C.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
diff --git a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
index bb9275a..077a7ff 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/ADXL345_SPI.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
diff --git a/wpilibc/src/main/native/cpp/ADXL362.cpp b/wpilibc/src/main/native/cpp/ADXL362.cpp
index 2f97f1b..6ed8c8c 100644
--- a/wpilibc/src/main/native/cpp/ADXL362.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL362.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/ADXL362.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/DriverStation.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -72,7 +72,7 @@
   commands[2] = kPowerCtl_Measure | kPowerCtl_UltraLowNoise;
   m_spi.Write(commands, 3);
 
-  HAL_Report(HALUsageReporting::kResourceType_ADXL362, port);
+  HAL_Report(HALUsageReporting::kResourceType_ADXL362, port + 1);
 
   SendableRegistry::GetInstance().AddLW(this, "ADXL362", port);
 }
diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
index 3dde234..2cf2f73 100644
--- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
+++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/ADXRS450_Gyro.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/DriverStation.h"
 #include "frc/Timer.h"
@@ -57,7 +57,7 @@
     Calibrate();
   }
 
-  HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port);
+  HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port + 1);
 
   SendableRegistry::GetInstance().AddLW(this, "ADXRS450_Gyro", port);
 }
diff --git a/wpilibc/src/main/native/cpp/AddressableLED.cpp b/wpilibc/src/main/native/cpp/AddressableLED.cpp
new file mode 100644
index 0000000..b0c3a45
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AddressableLED.cpp
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AddressableLED.h"
+
+#include <hal/AddressableLED.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/PWM.h>
+#include <hal/Ports.h>
+
+#include "frc/WPIErrors.h"
+
+using namespace frc;
+
+AddressableLED::AddressableLED(int port) {
+  int32_t status = 0;
+
+  m_pwmHandle = HAL_InitializePWMPort(HAL_GetPort(port), &status);
+  wpi_setHALErrorWithRange(status, 0, HAL_GetNumPWMChannels(), port);
+  if (m_pwmHandle == HAL_kInvalidHandle) {
+    return;
+  }
+
+  m_handle = HAL_InitializeAddressableLED(m_pwmHandle, &status);
+  wpi_setHALError(status);
+  if (m_handle == HAL_kInvalidHandle) {
+    HAL_FreePWMPort(m_pwmHandle, &status);
+  }
+
+  HAL_Report(HALUsageReporting::kResourceType_AddressableLEDs, port + 1);
+}
+
+AddressableLED::~AddressableLED() {
+  HAL_FreeAddressableLED(m_handle);
+  int32_t status = 0;
+  HAL_FreePWMPort(m_pwmHandle, &status);
+}
+
+void AddressableLED::SetLength(int length) {
+  int32_t status = 0;
+  HAL_SetAddressableLEDLength(m_handle, length, &status);
+  wpi_setHALError(status);
+}
+
+static_assert(sizeof(AddressableLED::LEDData) == sizeof(HAL_AddressableLEDData),
+              "LED Structs MUST be the same size");
+
+void AddressableLED::SetData(wpi::ArrayRef<LEDData> ledData) {
+  int32_t status = 0;
+  HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
+                              &status);
+  wpi_setHALError(status);
+}
+
+void AddressableLED::SetData(std::initializer_list<LEDData> ledData) {
+  int32_t status = 0;
+  HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
+                              &status);
+  wpi_setHALError(status);
+}
+
+void AddressableLED::SetBitTiming(units::nanosecond_t lowTime0,
+                                  units::nanosecond_t highTime0,
+                                  units::nanosecond_t lowTime1,
+                                  units::nanosecond_t highTime1) {
+  int32_t status = 0;
+  HAL_SetAddressableLEDBitTiming(
+      m_handle, lowTime0.to<int32_t>(), highTime0.to<int32_t>(),
+      lowTime1.to<int32_t>(), highTime1.to<int32_t>(), &status);
+  wpi_setHALError(status);
+}
+
+void AddressableLED::SetSyncTime(units::microsecond_t syncTime) {
+  int32_t status = 0;
+  HAL_SetAddressableLEDSyncTime(m_handle, syncTime.to<int32_t>(), &status);
+  wpi_setHALError(status);
+}
+
+void AddressableLED::Start() {
+  int32_t status = 0;
+  HAL_StartAddressableLEDOutput(m_handle, &status);
+  wpi_setHALError(status);
+}
+
+void AddressableLED::Stop() {
+  int32_t status = 0;
+  HAL_StopAddressableLEDOutput(m_handle, &status);
+  wpi_setHALError(status);
+}
+
+void AddressableLED::LEDData::SetHSV(int h, int s, int v) {
+  if (s == 0) {
+    SetRGB(v, v, v);
+    return;
+  }
+
+  int region = h / 30;
+  int remainder = (h - (region * 30)) * 6;
+
+  int p = (v * (255 - s)) >> 8;
+  int q = (v * (255 - ((s * remainder) >> 8))) >> 8;
+  int t = (v * (255 - ((s * (255 - remainder)) >> 8))) >> 8;
+
+  switch (region) {
+    case 0:
+      SetRGB(v, t, p);
+      break;
+    case 1:
+      SetRGB(q, v, p);
+      break;
+    case 2:
+      SetRGB(p, v, t);
+      break;
+    case 3:
+      SetRGB(p, q, v);
+      break;
+    case 4:
+      SetRGB(t, p, v);
+      break;
+    default:
+      SetRGB(v, p, q);
+      break;
+  }
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
index 4436bcf..be0c7d2 100644
--- a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/AnalogAccelerometer.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -58,7 +58,7 @@
 
 void AnalogAccelerometer::InitAccelerometer() {
   HAL_Report(HALUsageReporting::kResourceType_Accelerometer,
-             m_analogInput->GetChannel());
+             m_analogInput->GetChannel() + 1);
 
   SendableRegistry::GetInstance().AddLW(this, "Accelerometer",
                                         m_analogInput->GetChannel());
diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
new file mode 100644
index 0000000..a194961
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/AnalogEncoder.h"
+
+#include "frc/AnalogInput.h"
+#include "frc/Counter.h"
+#include "frc/DriverStation.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+AnalogEncoder::AnalogEncoder(AnalogInput& analogInput)
+    : m_analogInput{&analogInput, NullDeleter<AnalogInput>{}},
+      m_analogTrigger{m_analogInput.get()},
+      m_counter{} {
+  Init();
+}
+
+AnalogEncoder::AnalogEncoder(AnalogInput* analogInput)
+    : m_analogInput{analogInput, NullDeleter<AnalogInput>{}},
+      m_analogTrigger{m_analogInput.get()},
+      m_counter{} {
+  Init();
+}
+
+AnalogEncoder::AnalogEncoder(std::shared_ptr<AnalogInput> analogInput)
+    : m_analogInput{std::move(analogInput)},
+      m_analogTrigger{m_analogInput.get()},
+      m_counter{} {
+  Init();
+}
+
+void AnalogEncoder::Init() {
+  m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()};
+
+  if (m_simDevice) {
+    m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
+  }
+
+  m_analogTrigger.SetLimitsVoltage(1.25, 3.75);
+  m_counter.SetUpSource(
+      m_analogTrigger.CreateOutput(AnalogTriggerType::kRisingPulse));
+  m_counter.SetDownSource(
+      m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse));
+
+  SendableRegistry::GetInstance().AddLW(this, "DutyCycle Encoder",
+                                        m_analogInput->GetChannel());
+}
+
+units::turn_t AnalogEncoder::Get() const {
+  if (m_simPosition) return units::turn_t{m_simPosition.Get()};
+
+  // As the values are not atomic, keep trying until we get 2 reads of the same
+  // value If we don't within 10 attempts, error
+  for (int i = 0; i < 10; i++) {
+    auto counter = m_counter.Get();
+    auto pos = m_analogInput->GetVoltage();
+    auto counter2 = m_counter.Get();
+    auto pos2 = m_analogInput->GetVoltage();
+    if (counter == counter2 && pos == pos2) {
+      units::turn_t turns{counter + pos - m_positionOffset};
+      m_lastPosition = turns;
+      return turns;
+    }
+  }
+
+  frc::DriverStation::GetInstance().ReportWarning(
+      "Failed to read Analog Encoder. Potential Speed Overrun. Returning last "
+      "value");
+  return m_lastPosition;
+}
+
+double AnalogEncoder::GetPositionOffset() const { return m_positionOffset; }
+
+void AnalogEncoder::SetDistancePerRotation(double distancePerRotation) {
+  m_distancePerRotation = distancePerRotation;
+}
+
+double AnalogEncoder::GetDistancePerRotation() const {
+  return m_distancePerRotation;
+}
+
+double AnalogEncoder::GetDistance() const {
+  return Get().to<double>() * GetDistancePerRotation();
+}
+
+void AnalogEncoder::Reset() {
+  m_counter.Reset();
+  m_positionOffset = m_analogInput->GetVoltage();
+}
+
+void AnalogEncoder::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("AbsoluteEncoder");
+  builder.AddDoubleProperty("Distance", [this] { return this->GetDistance(); },
+                            nullptr);
+  builder.AddDoubleProperty("Distance Per Rotation",
+                            [this] { return this->GetDistancePerRotation(); },
+                            nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogGyro.cpp b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
index 36cde8a..c891506 100644
--- a/wpilibc/src/main/native/cpp/AnalogGyro.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
@@ -12,7 +12,7 @@
 
 #include <hal/AnalogGyro.h>
 #include <hal/Errors.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/AnalogInput.h"
 #include "frc/Timer.h"
@@ -56,7 +56,7 @@
     HAL_SetAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
                                 offset, center, &status);
     if (status != 0) {
-      wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+      wpi_setHALError(status);
       m_gyroHandle = HAL_kInvalidHandle;
       return;
     }
@@ -70,7 +70,7 @@
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetAnalogGyroAngle(m_gyroHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -78,7 +78,7 @@
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetAnalogGyroRate(m_gyroHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -86,7 +86,7 @@
   if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetAnalogGyroCenter(m_gyroHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -94,7 +94,7 @@
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetAnalogGyroOffset(m_gyroHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -102,21 +102,21 @@
   int32_t status = 0;
   HAL_SetAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle,
                                            voltsPerDegreePerSecond, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void AnalogGyro::SetDeadband(double volts) {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogGyroDeadband(m_gyroHandle, volts, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void AnalogGyro::Reset() {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_ResetAnalogGyro(m_gyroHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void AnalogGyro::InitGyro() {
@@ -132,7 +132,7 @@
       return;
     }
     if (status != 0) {
-      wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+      wpi_setHALError(status);
       m_analog = nullptr;
       m_gyroHandle = HAL_kInvalidHandle;
       return;
@@ -142,13 +142,13 @@
   int32_t status = 0;
   HAL_SetupAnalogGyro(m_gyroHandle, &status);
   if (status != 0) {
-    wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+    wpi_setHALError(status);
     m_analog = nullptr;
     m_gyroHandle = HAL_kInvalidHandle;
     return;
   }
 
-  HAL_Report(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel() + 1);
 
   SendableRegistry::GetInstance().AddLW(this, "AnalogGyro",
                                         m_analog->GetChannel());
@@ -158,5 +158,5 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_CalibrateAnalogGyro(m_gyroHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
diff --git a/wpilibc/src/main/native/cpp/AnalogInput.cpp b/wpilibc/src/main/native/cpp/AnalogInput.cpp
index a200f4d..c8197b7 100644
--- a/wpilibc/src/main/native/cpp/AnalogInput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogInput.cpp
@@ -11,7 +11,8 @@
 
 #include <hal/AnalogAccumulator.h>
 #include <hal/AnalogInput.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
 #include <hal/Ports.h>
 
 #include "frc/SensorUtil.h"
@@ -35,14 +36,13 @@
   int32_t status = 0;
   m_port = HAL_InitializeAnalogInputPort(port, &status);
   if (status != 0) {
-    wpi_setErrorWithContextRange(status, 0, HAL_GetNumAnalogInputs(), channel,
-                                 HAL_GetErrorMessage(status));
+    wpi_setHALErrorWithRange(status, 0, HAL_GetNumAnalogInputs(), channel);
     m_channel = std::numeric_limits<int>::max();
     m_port = HAL_kInvalidHandle;
     return;
   }
 
-  HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel);
+  HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel + 1);
 
   SendableRegistry::GetInstance().AddLW(this, "AnalogInput", channel);
 }
@@ -53,7 +53,7 @@
   if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetAnalogValue(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -61,7 +61,7 @@
   if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetAnalogAverageValue(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -69,7 +69,7 @@
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double voltage = HAL_GetAnalogVoltage(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return voltage;
 }
 
@@ -77,7 +77,7 @@
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double voltage = HAL_GetAnalogAverageVoltage(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return voltage;
 }
 
@@ -90,13 +90,13 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogAverageBits(m_port, bits, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int AnalogInput::GetAverageBits() const {
   int32_t status = 0;
   int averageBits = HAL_GetAnalogAverageBits(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return averageBits;
 }
 
@@ -104,14 +104,14 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogOversampleBits(m_port, bits, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int AnalogInput::GetOversampleBits() const {
   if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int oversampleBits = HAL_GetAnalogOversampleBits(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return oversampleBits;
 }
 
@@ -119,7 +119,7 @@
   if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int lsbWeight = HAL_GetAnalogLSBWeight(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return lsbWeight;
 }
 
@@ -127,7 +127,7 @@
   if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int offset = HAL_GetAnalogOffset(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return offset;
 }
 
@@ -135,7 +135,7 @@
   if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool isAccum = HAL_IsAccumulatorChannel(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return isAccum;
 }
 
@@ -144,7 +144,7 @@
   m_accumulatorOffset = 0;
   int32_t status = 0;
   HAL_InitAccumulator(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void AnalogInput::SetAccumulatorInitialValue(int64_t initialValue) {
@@ -156,7 +156,7 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_ResetAccumulator(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   if (!StatusIsFatal()) {
     // Wait until the next sample, so the next call to GetAccumulator*()
@@ -172,21 +172,21 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAccumulatorCenter(m_port, center, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void AnalogInput::SetAccumulatorDeadband(int deadband) {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAccumulatorDeadband(m_port, deadband, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int64_t AnalogInput::GetAccumulatorValue() const {
   if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int64_t value = HAL_GetAccumulatorValue(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value + m_accumulatorOffset;
 }
 
@@ -194,7 +194,7 @@
   if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int64_t count = HAL_GetAccumulatorCount(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return count;
 }
 
@@ -202,20 +202,20 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_GetAccumulatorOutput(m_port, &value, &count, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   value += m_accumulatorOffset;
 }
 
 void AnalogInput::SetSampleRate(double samplesPerSecond) {
   int32_t status = 0;
   HAL_SetAnalogSampleRate(samplesPerSecond, &status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
 }
 
 double AnalogInput::GetSampleRate() {
   int32_t status = 0;
   double sampleRate = HAL_GetAnalogSampleRate(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return sampleRate;
 }
 
diff --git a/wpilibc/src/main/native/cpp/AnalogOutput.cpp b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
index 5e56efc..041b446 100644
--- a/wpilibc/src/main/native/cpp/AnalogOutput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
@@ -10,7 +10,9 @@
 #include <limits>
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/AnalogOutput.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
 #include <hal/Ports.h>
 
 #include "frc/SensorUtil.h"
@@ -35,14 +37,13 @@
   int32_t status = 0;
   m_port = HAL_InitializeAnalogOutputPort(port, &status);
   if (status != 0) {
-    wpi_setErrorWithContextRange(status, 0, HAL_GetNumAnalogOutputs(), channel,
-                                 HAL_GetErrorMessage(status));
+    wpi_setHALErrorWithRange(status, 0, HAL_GetNumAnalogOutputs(), channel);
     m_channel = std::numeric_limits<int>::max();
     m_port = HAL_kInvalidHandle;
     return;
   }
 
-  HAL_Report(HALUsageReporting::kResourceType_AnalogOutput, m_channel);
+  HAL_Report(HALUsageReporting::kResourceType_AnalogOutput, m_channel + 1);
   SendableRegistry::GetInstance().AddLW(this, "AnalogOutput", m_channel);
 }
 
@@ -52,14 +53,14 @@
   int32_t status = 0;
   HAL_SetAnalogOutput(m_port, voltage, &status);
 
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 double AnalogOutput::GetVoltage() const {
   int32_t status = 0;
   double voltage = HAL_GetAnalogOutput(m_port, &status);
 
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   return voltage;
 }
diff --git a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
index 60ce04a..ddbb7c8 100644
--- a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
@@ -9,9 +9,10 @@
 
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/AnalogInput.h"
+#include "frc/DutyCycle.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableRegistry.h"
 
@@ -26,19 +27,31 @@
 AnalogTrigger::AnalogTrigger(AnalogInput* input) {
   m_analogInput = input;
   int32_t status = 0;
-  int index = 0;
-  m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &index, &status);
+  m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &status);
   if (status != 0) {
-    wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
-    m_index = std::numeric_limits<int>::max();
+    wpi_setHALError(status);
     m_trigger = HAL_kInvalidHandle;
     return;
   }
-  m_index = index;
+  int index = GetIndex();
 
-  HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, input->m_channel);
-  SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger",
-                                        input->GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
+  SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger", index);
+}
+
+AnalogTrigger::AnalogTrigger(DutyCycle* input) {
+  m_dutyCycle = input;
+  int32_t status = 0;
+  m_trigger = HAL_InitializeAnalogTriggerDutyCycle(input->m_handle, &status);
+  if (status != 0) {
+    wpi_setHALError(status);
+    m_trigger = HAL_kInvalidHandle;
+    return;
+  }
+  int index = GetIndex();
+
+  HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
+  SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger", index);
 }
 
 AnalogTrigger::~AnalogTrigger() {
@@ -53,9 +66,9 @@
 AnalogTrigger::AnalogTrigger(AnalogTrigger&& rhs)
     : ErrorBase(std::move(rhs)),
       SendableHelper(std::move(rhs)),
-      m_index(std::move(rhs.m_index)),
       m_trigger(std::move(rhs.m_trigger)) {
   std::swap(m_analogInput, rhs.m_analogInput);
+  std::swap(m_dutyCycle, rhs.m_dutyCycle);
   std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
 }
 
@@ -63,9 +76,9 @@
   ErrorBase::operator=(std::move(rhs));
   SendableHelper::operator=(std::move(rhs));
 
-  m_index = std::move(rhs.m_index);
   m_trigger = std::move(rhs.m_trigger);
   std::swap(m_analogInput, rhs.m_analogInput);
+  std::swap(m_dutyCycle, rhs.m_dutyCycle);
   std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
 
   return *this;
@@ -75,40 +88,50 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogTriggerLimitsVoltage(m_trigger, lower, upper, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
+}
+
+void AnalogTrigger::SetLimitsDutyCycle(double lower, double upper) {
+  if (StatusIsFatal()) return;
+  int32_t status = 0;
+  HAL_SetAnalogTriggerLimitsDutyCycle(m_trigger, lower, upper, &status);
+  wpi_setHALError(status);
 }
 
 void AnalogTrigger::SetLimitsRaw(int lower, int upper) {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogTriggerLimitsRaw(m_trigger, lower, upper, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void AnalogTrigger::SetAveraged(bool useAveragedValue) {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogTriggerAveraged(m_trigger, useAveragedValue, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void AnalogTrigger::SetFiltered(bool useFilteredValue) {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogTriggerFiltered(m_trigger, useFilteredValue, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int AnalogTrigger::GetIndex() const {
   if (StatusIsFatal()) return -1;
-  return m_index;
+  int32_t status = 0;
+  auto ret = HAL_GetAnalogTriggerFPGAIndex(m_trigger, &status);
+  wpi_setHALError(status);
+  return ret;
 }
 
 bool AnalogTrigger::GetInWindow() {
   if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool result = HAL_GetAnalogTriggerInWindow(m_trigger, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return result;
 }
 
@@ -116,7 +139,7 @@
   if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool result = HAL_GetAnalogTriggerTriggerState(m_trigger, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return result;
 }
 
diff --git a/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
index c2c7265..8fba479 100644
--- a/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/AnalogTriggerOutput.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/AnalogTrigger.h"
 #include "frc/WPIErrors.h"
@@ -19,7 +19,7 @@
   bool result = HAL_GetAnalogTriggerOutput(
       m_trigger->m_trigger, static_cast<HAL_AnalogTriggerType>(m_outputType),
       &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return result;
 }
 
@@ -33,7 +33,7 @@
 
 bool AnalogTriggerOutput::IsAnalogTrigger() const { return true; }
 
-int AnalogTriggerOutput::GetChannel() const { return m_trigger->m_index; }
+int AnalogTriggerOutput::GetChannel() const { return m_trigger->GetIndex(); }
 
 void AnalogTriggerOutput::InitSendable(SendableBuilder&) {}
 
@@ -41,5 +41,5 @@
                                          AnalogTriggerType outputType)
     : m_trigger(&trigger), m_outputType(outputType) {
   HAL_Report(HALUsageReporting::kResourceType_AnalogTriggerOutput,
-             trigger.GetIndex(), static_cast<uint8_t>(outputType));
+             trigger.GetIndex() + 1, static_cast<uint8_t>(outputType) + 1);
 }
diff --git a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
index 71c8e89..d7cdef6 100644
--- a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
@@ -8,7 +8,7 @@
 #include "frc/BuiltInAccelerometer.h"
 
 #include <hal/Accelerometer.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableBuilder.h"
diff --git a/wpilibc/src/main/native/cpp/CAN.cpp b/wpilibc/src/main/native/cpp/CAN.cpp
index 35a926a..797b9ff 100644
--- a/wpilibc/src/main/native/cpp/CAN.cpp
+++ b/wpilibc/src/main/native/cpp/CAN.cpp
@@ -13,7 +13,6 @@
 #include <hal/CANAPI.h>
 #include <hal/Errors.h>
 #include <hal/FRCUsageReporting.h>
-#include <hal/HALBase.h>
 
 using namespace frc;
 
@@ -22,12 +21,12 @@
   m_handle =
       HAL_InitializeCAN(kTeamManufacturer, deviceId, kTeamDeviceType, &status);
   if (status != 0) {
-    wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+    wpi_setHALError(status);
     m_handle = HAL_kInvalidHandle;
     return;
   }
 
-  HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId);
+  HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId + 1);
 }
 
 CAN::CAN(int deviceId, int deviceManufacturer, int deviceType) {
@@ -36,12 +35,12 @@
       static_cast<HAL_CANManufacturer>(deviceManufacturer), deviceId,
       static_cast<HAL_CANDeviceType>(deviceType), &status);
   if (status != 0) {
-    wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+    wpi_setHALError(status);
     m_handle = HAL_kInvalidHandle;
     return;
   }
 
-  HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId);
+  HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId + 1);
 }
 
 CAN::~CAN() {
@@ -55,26 +54,26 @@
 void CAN::WritePacket(const uint8_t* data, int length, int apiId) {
   int32_t status = 0;
   HAL_WriteCANPacket(m_handle, data, length, apiId, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void CAN::WritePacketRepeating(const uint8_t* data, int length, int apiId,
                                int repeatMs) {
   int32_t status = 0;
   HAL_WriteCANPacketRepeating(m_handle, data, length, apiId, repeatMs, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void CAN::WriteRTRFrame(int length, int apiId) {
   int32_t status = 0;
   HAL_WriteCANRTRFrame(m_handle, length, apiId, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void CAN::StopPacketRepeating(int apiId) {
   int32_t status = 0;
   HAL_StopCANPacketRepeating(m_handle, apiId, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 bool CAN::ReadPacketNew(int apiId, CANData* data) {
@@ -85,7 +84,7 @@
     return false;
   }
   if (status != 0) {
-    wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+    wpi_setHALError(status);
     return false;
   } else {
     return true;
@@ -100,7 +99,7 @@
     return false;
   }
   if (status != 0) {
-    wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+    wpi_setHALError(status);
     return false;
   } else {
     return true;
@@ -116,7 +115,7 @@
     return false;
   }
   if (status != 0) {
-    wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+    wpi_setHALError(status);
     return false;
   } else {
     return true;
diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp
index 10d75f1..29789be 100644
--- a/wpilibc/src/main/native/cpp/Compressor.cpp
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -8,7 +8,7 @@
 #include "frc/Compressor.h"
 
 #include <hal/Compressor.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <hal/Ports.h>
 #include <hal/Solenoid.h>
 
@@ -22,13 +22,12 @@
   int32_t status = 0;
   m_compressorHandle = HAL_InitializeCompressor(m_module, &status);
   if (status != 0) {
-    wpi_setErrorWithContextRange(status, 0, HAL_GetNumPCMModules(), pcmID,
-                                 HAL_GetErrorMessage(status));
+    wpi_setHALErrorWithRange(status, 0, HAL_GetNumPCMModules(), pcmID);
     return;
   }
   SetClosedLoopControl(true);
 
-  HAL_Report(HALUsageReporting::kResourceType_Compressor, pcmID);
+  HAL_Report(HALUsageReporting::kResourceType_Compressor, pcmID + 1);
   SendableRegistry::GetInstance().AddLW(this, "Compressor", pcmID);
 }
 
diff --git a/wpilibc/src/main/native/cpp/Counter.cpp b/wpilibc/src/main/native/cpp/Counter.cpp
index 1ef40fd..d2158ca 100644
--- a/wpilibc/src/main/native/cpp/Counter.cpp
+++ b/wpilibc/src/main/native/cpp/Counter.cpp
@@ -9,7 +9,8 @@
 
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/AnalogTrigger.h"
 #include "frc/DigitalInput.h"
@@ -22,11 +23,11 @@
 Counter::Counter(Mode mode) {
   int32_t status = 0;
   m_counter = HAL_InitializeCounter((HAL_Counter_Mode)mode, &m_index, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
-  SetMaxPeriod(.5);
+  SetMaxPeriod(0.5);
 
-  HAL_Report(HALUsageReporting::kResourceType_Counter, m_index, mode);
+  HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1, mode + 1);
   SendableRegistry::GetInstance().AddLW(this, "Counter", m_index);
 }
 
@@ -81,7 +82,7 @@
     HAL_SetCounterAverageSize(m_counter, 2, &status);
   }
 
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   SetDownSourceEdge(inverted, true);
 }
 
@@ -90,7 +91,7 @@
 
   int32_t status = 0;
   HAL_FreeCounter(m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Counter::SetUpSource(int channel) {
@@ -128,7 +129,7 @@
         m_counter, source->GetPortHandleForRouting(),
         (HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(),
         &status);
-    wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+    wpi_setHALError(status);
   }
 }
 
@@ -146,7 +147,7 @@
   }
   int32_t status = 0;
   HAL_SetCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Counter::ClearUpSource() {
@@ -154,7 +155,7 @@
   m_upSource.reset();
   int32_t status = 0;
   HAL_ClearCounterUpSource(m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Counter::SetDownSource(int channel) {
@@ -197,7 +198,7 @@
         m_counter, source->GetPortHandleForRouting(),
         (HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(),
         &status);
-    wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+    wpi_setHALError(status);
   }
 }
 
@@ -210,7 +211,7 @@
   }
   int32_t status = 0;
   HAL_SetCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Counter::ClearDownSource() {
@@ -218,42 +219,42 @@
   m_downSource.reset();
   int32_t status = 0;
   HAL_ClearCounterDownSource(m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Counter::SetUpDownCounterMode() {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterUpDownMode(m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Counter::SetExternalDirectionMode() {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterExternalDirectionMode(m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterSemiPeriodMode(m_counter, highSemiPeriod, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Counter::SetPulseLengthMode(double threshold) {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterPulseLengthMode(m_counter, threshold, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Counter::SetReverseDirection(bool reverseDirection) {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterReverseDirection(m_counter, reverseDirection, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Counter::SetSamplesToAverage(int samplesToAverage) {
@@ -264,13 +265,13 @@
   }
   int32_t status = 0;
   HAL_SetCounterSamplesToAverage(m_counter, samplesToAverage, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int Counter::GetSamplesToAverage() const {
   int32_t status = 0;
   int samples = HAL_GetCounterSamplesToAverage(m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return samples;
 }
 
@@ -280,7 +281,7 @@
   if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetCounter(m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -288,14 +289,14 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_ResetCounter(m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 double Counter::GetPeriod() const {
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetCounterPeriod(m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -303,21 +304,21 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterMaxPeriod(m_counter, maxPeriod, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Counter::SetUpdateWhenEmpty(bool enabled) {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterUpdateWhenEmpty(m_counter, enabled, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 bool Counter::GetStopped() const {
   if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool value = HAL_GetCounterStopped(m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -325,7 +326,7 @@
   if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool value = HAL_GetCounterDirection(m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
diff --git a/wpilibc/src/main/native/cpp/DMA.cpp b/wpilibc/src/main/native/cpp/DMA.cpp
new file mode 100644
index 0000000..1861555
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DMA.cpp
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DMA.h"
+
+#include <frc/AnalogInput.h>
+#include <frc/Counter.h>
+#include <frc/DigitalSource.h>
+#include <frc/DutyCycle.h>
+#include <frc/Encoder.h>
+
+#include <hal/DMA.h>
+#include <hal/HALBase.h>
+
+using namespace frc;
+
+DMA::DMA() {
+  int32_t status = 0;
+  dmaHandle = HAL_InitializeDMA(&status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+DMA::~DMA() { HAL_FreeDMA(dmaHandle); }
+
+void DMA::SetPause(bool pause) {
+  int32_t status = 0;
+  HAL_SetDMAPause(dmaHandle, pause, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::SetRate(int cycles) {
+  int32_t status = 0;
+  HAL_SetDMARate(dmaHandle, cycles, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddEncoder(const Encoder* encoder) {
+  int32_t status = 0;
+  HAL_AddDMAEncoder(dmaHandle, encoder->m_encoder, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddEncoderPeriod(const Encoder* encoder) {
+  int32_t status = 0;
+  HAL_AddDMAEncoderPeriod(dmaHandle, encoder->m_encoder, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddCounter(const Counter* counter) {
+  int32_t status = 0;
+  HAL_AddDMACounter(dmaHandle, counter->m_counter, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddCounterPeriod(const Counter* counter) {
+  int32_t status = 0;
+  HAL_AddDMACounterPeriod(dmaHandle, counter->m_counter, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddDigitalSource(const DigitalSource* digitalSource) {
+  int32_t status = 0;
+  HAL_AddDMADigitalSource(dmaHandle, digitalSource->GetPortHandleForRouting(),
+                          &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddDutyCycle(const DutyCycle* dutyCycle) {
+  int32_t status = 0;
+  HAL_AddDMADutyCycle(dmaHandle, dutyCycle->m_handle, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddAnalogInput(const AnalogInput* analogInput) {
+  int32_t status = 0;
+  HAL_AddDMAAnalogInput(dmaHandle, analogInput->m_port, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddAveragedAnalogInput(const AnalogInput* analogInput) {
+  int32_t status = 0;
+  HAL_AddDMAAveragedAnalogInput(dmaHandle, analogInput->m_port, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::AddAnalogAccumulator(const AnalogInput* analogInput) {
+  int32_t status = 0;
+  HAL_AddDMAAnalogAccumulator(dmaHandle, analogInput->m_port, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::SetExternalTrigger(DigitalSource* source, bool rising, bool falling) {
+  int32_t status = 0;
+  HAL_SetDMAExternalTrigger(dmaHandle, source->GetPortHandleForRouting(),
+                            static_cast<HAL_AnalogTriggerType>(
+                                source->GetAnalogTriggerTypeForRouting()),
+                            rising, falling, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::StartDMA(int queueDepth) {
+  int32_t status = 0;
+  HAL_StartDMA(dmaHandle, queueDepth, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
+
+void DMA::StopDMA() {
+  int32_t status = 0;
+  HAL_StopDMA(dmaHandle, &status);
+  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+}
diff --git a/wpilibc/src/main/native/cpp/DMC60.cpp b/wpilibc/src/main/native/cpp/DMC60.cpp
index d61bd40..e7d2c65 100644
--- a/wpilibc/src/main/native/cpp/DMC60.cpp
+++ b/wpilibc/src/main/native/cpp/DMC60.cpp
@@ -7,32 +7,18 @@
 
 #include "frc/DMC60.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 DMC60::DMC60(int channel) : PWMSpeedController(channel) {
-  /*
-   * Note that the DMC 60 uses the following bounds for PWM values. These
-   * values should work reasonably well for most controllers, but if users
-   * experience issues such as asymmetric behavior around the deadband or
-   * inability to saturate the controller in either direction, calibration is
-   * recommended. The calibration procedure can be found in the DMC 60 User
-   * Manual available from Digilent.
-   *
-   *   2.004ms = full "forward"
-   *   1.52ms = the "high end" of the deadband range
-   *   1.50ms = center of the deadband range (off)
-   *   1.48ms = the "low end" of the deadband range
-   *   0.997ms = full "reverse"
-   */
-  SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
   SetPeriodMultiplier(kPeriodMultiplier_1X);
   SetSpeed(0.0);
   SetZeroLatch();
 
-  HAL_Report(HALUsageReporting::kResourceType_DigilentDMC60, GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_DigilentDMC60, GetChannel() + 1);
   SendableRegistry::GetInstance().SetName(this, "DMC60", GetChannel());
 }
diff --git a/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
index 9e92da8..3cec3f1 100644
--- a/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
@@ -13,7 +13,7 @@
 
 #include <hal/Constants.h>
 #include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/Counter.h"
 #include "frc/Encoder.h"
@@ -38,7 +38,7 @@
   *index = true;
 
   HAL_Report(HALUsageReporting::kResourceType_DigitalGlitchFilter,
-             m_channelIndex);
+             m_channelIndex + 1);
   SendableRegistry::GetInstance().AddLW(this, "DigitalGlitchFilter",
                                         m_channelIndex);
 }
@@ -81,15 +81,12 @@
     int32_t status = 0;
     HAL_SetFilterSelect(input->GetPortHandleForRouting(), requestedIndex,
                         &status);
-    wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+    wpi_setHALError(status);
 
     // Validate that we set it correctly.
     int actualIndex =
         HAL_GetFilterSelect(input->GetPortHandleForRouting(), &status);
     wpi_assertEqual(actualIndex, requestedIndex);
-
-    HAL_Report(HALUsageReporting::kResourceType_DigitalInput,
-               input->GetChannel());
   }
 }
 
@@ -130,7 +127,7 @@
 void DigitalGlitchFilter::SetPeriodCycles(int fpgaCycles) {
   int32_t status = 0;
   HAL_SetFilterPeriod(m_channelIndex, fpgaCycles, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void DigitalGlitchFilter::SetPeriodNanoSeconds(uint64_t nanoseconds) {
@@ -139,14 +136,14 @@
       nanoseconds * HAL_GetSystemClockTicksPerMicrosecond() / 4 / 1000;
   HAL_SetFilterPeriod(m_channelIndex, fpgaCycles, &status);
 
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int DigitalGlitchFilter::GetPeriodCycles() {
   int32_t status = 0;
   int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
 
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   return fpgaCycles;
 }
@@ -155,7 +152,7 @@
   int32_t status = 0;
   int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
 
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   return static_cast<uint64_t>(fpgaCycles) * 1000L /
          static_cast<uint64_t>(HAL_GetSystemClockTicksPerMicrosecond() / 4);
diff --git a/wpilibc/src/main/native/cpp/DigitalInput.cpp b/wpilibc/src/main/native/cpp/DigitalInput.cpp
index d0fa41b..7210750 100644
--- a/wpilibc/src/main/native/cpp/DigitalInput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -11,7 +11,8 @@
 #include <utility>
 
 #include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
 #include <hal/Ports.h>
 
 #include "frc/SensorUtil.h"
@@ -33,14 +34,13 @@
   int32_t status = 0;
   m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), true, &status);
   if (status != 0) {
-    wpi_setErrorWithContextRange(status, 0, HAL_GetNumDigitalChannels(),
-                                 channel, HAL_GetErrorMessage(status));
+    wpi_setHALErrorWithRange(status, 0, HAL_GetNumDigitalChannels(), channel);
     m_handle = HAL_kInvalidHandle;
     m_channel = std::numeric_limits<int>::max();
     return;
   }
 
-  HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel);
+  HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel + 1);
   SendableRegistry::GetInstance().AddLW(this, "DigitalInput", channel);
 }
 
@@ -53,7 +53,7 @@
   if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool value = HAL_GetDIO(m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
diff --git a/wpilibc/src/main/native/cpp/DigitalOutput.cpp b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
index 2068b5d..6e9b09b 100644
--- a/wpilibc/src/main/native/cpp/DigitalOutput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
@@ -11,7 +11,8 @@
 #include <utility>
 
 #include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
 #include <hal/Ports.h>
 
 #include "frc/SensorUtil.h"
@@ -34,14 +35,13 @@
   int32_t status = 0;
   m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), false, &status);
   if (status != 0) {
-    wpi_setErrorWithContextRange(status, 0, HAL_GetNumDigitalChannels(),
-                                 channel, HAL_GetErrorMessage(status));
+    wpi_setHALErrorWithRange(status, 0, HAL_GetNumDigitalChannels(), channel);
     m_channel = std::numeric_limits<int>::max();
     m_handle = HAL_kInvalidHandle;
     return;
   }
 
-  HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel);
+  HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel + 1);
   SendableRegistry::GetInstance().AddLW(this, "DigitalOutput", channel);
 }
 
@@ -58,7 +58,7 @@
 
   int32_t status = 0;
   HAL_SetDIO(m_handle, value, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 bool DigitalOutput::Get() const {
@@ -66,10 +66,18 @@
 
   int32_t status = 0;
   bool val = HAL_GetDIO(m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return val;
 }
 
+HAL_Handle DigitalOutput::GetPortHandleForRouting() const { return m_handle; }
+
+AnalogTriggerType DigitalOutput::GetAnalogTriggerTypeForRouting() const {
+  return (AnalogTriggerType)0;
+}
+
+bool DigitalOutput::IsAnalogTrigger() const { return false; }
+
 int DigitalOutput::GetChannel() const { return m_channel; }
 
 void DigitalOutput::Pulse(double length) {
@@ -77,7 +85,7 @@
 
   int32_t status = 0;
   HAL_Pulse(m_handle, length, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 bool DigitalOutput::IsPulsing() const {
@@ -85,7 +93,7 @@
 
   int32_t status = 0;
   bool value = HAL_IsPulsing(m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -94,7 +102,7 @@
 
   int32_t status = 0;
   HAL_SetDigitalPWMRate(rate, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void DigitalOutput::EnablePWM(double initialDutyCycle) {
@@ -104,15 +112,15 @@
 
   if (StatusIsFatal()) return;
   m_pwmGenerator = HAL_AllocateDigitalPWM(&status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   if (StatusIsFatal()) return;
   HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, initialDutyCycle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   if (StatusIsFatal()) return;
   HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, m_channel, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void DigitalOutput::DisablePWM() {
@@ -124,10 +132,10 @@
   // Disable the output by routing to a dead bit.
   HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, SensorUtil::kDigitalChannels,
                                  &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   HAL_FreeDigitalPWM(m_pwmGenerator, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   m_pwmGenerator = HAL_kInvalidHandle;
 }
@@ -137,7 +145,7 @@
 
   int32_t status = 0;
   HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, dutyCycle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void DigitalOutput::SetSimDevice(HAL_SimDeviceHandle device) {
diff --git a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
index 0b35ff5..c896ea8 100644
--- a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
+++ b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
@@ -9,7 +9,8 @@
 
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
 #include <hal/Ports.h>
 #include <hal/Solenoid.h>
 
@@ -50,8 +51,8 @@
   m_forwardHandle = HAL_InitializeSolenoidPort(
       HAL_GetPortWithModule(moduleNumber, m_forwardChannel), &status);
   if (status != 0) {
-    wpi_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
-                                 forwardChannel, HAL_GetErrorMessage(status));
+    wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(),
+                             forwardChannel);
     m_forwardHandle = HAL_kInvalidHandle;
     m_reverseHandle = HAL_kInvalidHandle;
     return;
@@ -60,8 +61,8 @@
   m_reverseHandle = HAL_InitializeSolenoidPort(
       HAL_GetPortWithModule(moduleNumber, m_reverseChannel), &status);
   if (status != 0) {
-    wpi_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
-                                 reverseChannel, HAL_GetErrorMessage(status));
+    wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(),
+                             reverseChannel);
     // free forward solenoid
     HAL_FreeSolenoidPort(m_forwardHandle);
     m_forwardHandle = HAL_kInvalidHandle;
@@ -72,10 +73,10 @@
   m_forwardMask = 1 << m_forwardChannel;
   m_reverseMask = 1 << m_reverseChannel;
 
-  HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_forwardChannel,
-             m_moduleNumber);
-  HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel,
-             m_moduleNumber);
+  HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_forwardChannel + 1,
+             m_moduleNumber + 1);
+  HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel + 1,
+             m_moduleNumber + 1);
 
   SendableRegistry::GetInstance().AddLW(this, "DoubleSolenoid", m_moduleNumber,
                                         m_forwardChannel);
@@ -110,8 +111,8 @@
   int rstatus = 0;
   HAL_SetSolenoid(m_reverseHandle, reverse, &rstatus);
 
-  wpi_setErrorWithContext(fstatus, HAL_GetErrorMessage(fstatus));
-  wpi_setErrorWithContext(rstatus, HAL_GetErrorMessage(rstatus));
+  wpi_setHALError(fstatus);
+  wpi_setHALError(rstatus);
 }
 
 DoubleSolenoid::Value DoubleSolenoid::Get() const {
@@ -121,8 +122,8 @@
   bool valueForward = HAL_GetSolenoid(m_forwardHandle, &fstatus);
   bool valueReverse = HAL_GetSolenoid(m_reverseHandle, &rstatus);
 
-  wpi_setErrorWithContext(fstatus, HAL_GetErrorMessage(fstatus));
-  wpi_setErrorWithContext(rstatus, HAL_GetErrorMessage(rstatus));
+  wpi_setHALError(fstatus);
+  wpi_setHALError(rstatus);
 
   if (valueForward) return kForward;
   if (valueReverse) return kReverse;
diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp
index d268de7..7c64883 100644
--- a/wpilibc/src/main/native/cpp/DriverStation.cpp
+++ b/wpilibc/src/main/native/cpp/DriverStation.cpp
@@ -9,7 +9,9 @@
 
 #include <chrono>
 
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
 #include <hal/Power.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableEntry.h>
@@ -473,6 +475,13 @@
   return voltage;
 }
 
+void DriverStation::WakeupWaitForData() {
+  std::scoped_lock waitLock(m_waitForDataMutex);
+  // Nofify all threads
+  m_waitForDataCounter++;
+  m_waitForDataCond.notify_all();
+}
+
 void DriverStation::GetData() {
   {
     // Compute the pressed and released buttons
@@ -494,13 +503,7 @@
     }
   }
 
-  {
-    std::scoped_lock waitLock(m_waitForDataMutex);
-    // Nofify all threads
-    m_waitForDataCounter++;
-    m_waitForDataCond.notify_all();
-  }
-
+  WakeupWaitForData();
   SendMatchData();
 }
 
diff --git a/wpilibc/src/main/native/cpp/DutyCycle.cpp b/wpilibc/src/main/native/cpp/DutyCycle.cpp
new file mode 100644
index 0000000..12f390e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DutyCycle.cpp
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DutyCycle.h"
+
+#include <hal/DutyCycle.h>
+#include <hal/FRCUsageReporting.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/WPIErrors.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+DutyCycle::DutyCycle(DigitalSource* source)
+    : m_source{source, NullDeleter<DigitalSource>()} {
+  if (m_source == nullptr) {
+    wpi_setWPIError(NullParameter);
+  } else {
+    InitDutyCycle();
+  }
+}
+
+DutyCycle::DutyCycle(DigitalSource& source)
+    : m_source{&source, NullDeleter<DigitalSource>()} {
+  InitDutyCycle();
+}
+
+DutyCycle::DutyCycle(std::shared_ptr<DigitalSource> source)
+    : m_source{std::move(source)} {
+  if (m_source == nullptr) {
+    wpi_setWPIError(NullParameter);
+  } else {
+    InitDutyCycle();
+  }
+}
+
+DutyCycle::~DutyCycle() { HAL_FreeDutyCycle(m_handle); }
+
+void DutyCycle::InitDutyCycle() {
+  int32_t status = 0;
+  m_handle =
+      HAL_InitializeDutyCycle(m_source->GetPortHandleForRouting(),
+                              static_cast<HAL_AnalogTriggerType>(
+                                  m_source->GetAnalogTriggerTypeForRouting()),
+                              &status);
+  wpi_setHALError(status);
+  int index = GetFPGAIndex();
+  HAL_Report(HALUsageReporting::kResourceType_DutyCycle, index + 1);
+  SendableRegistry::GetInstance().AddLW(this, "Duty Cycle", index);
+}
+
+int DutyCycle::GetFPGAIndex() const {
+  int32_t status = 0;
+  auto retVal = HAL_GetDutyCycleFPGAIndex(m_handle, &status);
+  wpi_setHALError(status);
+  return retVal;
+}
+
+int DutyCycle::GetFrequency() const {
+  int32_t status = 0;
+  auto retVal = HAL_GetDutyCycleFrequency(m_handle, &status);
+  wpi_setHALError(status);
+  return retVal;
+}
+
+double DutyCycle::GetOutput() const {
+  int32_t status = 0;
+  auto retVal = HAL_GetDutyCycleOutput(m_handle, &status);
+  wpi_setHALError(status);
+  return retVal;
+}
+
+unsigned int DutyCycle::GetOutputRaw() const {
+  int32_t status = 0;
+  auto retVal = HAL_GetDutyCycleOutputRaw(m_handle, &status);
+  wpi_setHALError(status);
+  return retVal;
+}
+
+unsigned int DutyCycle::GetOutputScaleFactor() const {
+  int32_t status = 0;
+  auto retVal = HAL_GetDutyCycleOutputScaleFactor(m_handle, &status);
+  wpi_setHALError(status);
+  return retVal;
+}
+
+int DutyCycle::GetSourceChannel() const { return m_source->GetChannel(); }
+
+void DutyCycle::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Duty Cycle");
+  builder.AddDoubleProperty("Frequency",
+                            [this] { return this->GetFrequency(); }, nullptr);
+  builder.AddDoubleProperty("Output", [this] { return this->GetOutput(); },
+                            nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
new file mode 100644
index 0000000..728b1de
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -0,0 +1,150 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/DutyCycleEncoder.h"
+
+#include "frc/Counter.h"
+#include "frc/DigitalInput.h"
+#include "frc/DigitalSource.h"
+#include "frc/DriverStation.h"
+#include "frc/DutyCycle.h"
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+DutyCycleEncoder::DutyCycleEncoder(int channel)
+    : m_dutyCycle{std::make_shared<DutyCycle>(
+          std::make_shared<DigitalInput>(channel))},
+      m_analogTrigger{m_dutyCycle.get()},
+      m_counter{} {}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
+    : m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}},
+      m_analogTrigger{m_dutyCycle.get()},
+      m_counter{} {
+  Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle)
+    : m_dutyCycle{dutyCycle, NullDeleter<DutyCycle>{}},
+      m_analogTrigger{m_dutyCycle.get()},
+      m_counter{} {
+  Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle)
+    : m_dutyCycle{std::move(dutyCycle)},
+      m_analogTrigger{m_dutyCycle.get()},
+      m_counter{} {
+  Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource)
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
+      m_analogTrigger{m_dutyCycle.get()},
+      m_counter{} {
+  Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource)
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
+      m_analogTrigger{m_dutyCycle.get()},
+      m_counter{} {
+  Init();
+}
+
+DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource)
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
+      m_analogTrigger{m_dutyCycle.get()},
+      m_counter{} {
+  Init();
+}
+
+void DutyCycleEncoder::Init() {
+  m_simDevice = hal::SimDevice{"DutyCycleEncoder", m_dutyCycle->GetFPGAIndex()};
+
+  if (m_simDevice) {
+    m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
+    m_simIsConnected = m_simDevice.CreateBoolean("Connected", false, true);
+  }
+
+  m_analogTrigger.SetLimitsDutyCycle(0.25, 0.75);
+  m_counter.SetUpSource(
+      m_analogTrigger.CreateOutput(AnalogTriggerType::kRisingPulse));
+  m_counter.SetDownSource(
+      m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse));
+
+  SendableRegistry::GetInstance().AddLW(this, "DutyCycle Encoder",
+                                        m_dutyCycle->GetSourceChannel());
+}
+
+units::turn_t DutyCycleEncoder::Get() const {
+  if (m_simPosition) return units::turn_t{m_simPosition.Get()};
+
+  // As the values are not atomic, keep trying until we get 2 reads of the same
+  // value If we don't within 10 attempts, error
+  for (int i = 0; i < 10; i++) {
+    auto counter = m_counter.Get();
+    auto pos = m_dutyCycle->GetOutput();
+    auto counter2 = m_counter.Get();
+    auto pos2 = m_dutyCycle->GetOutput();
+    if (counter == counter2 && pos == pos2) {
+      units::turn_t turns{counter + pos - m_positionOffset};
+      m_lastPosition = turns;
+      return turns;
+    }
+  }
+
+  frc::DriverStation::GetInstance().ReportWarning(
+      "Failed to read DutyCycle Encoder. Potential Speed Overrun. Returning "
+      "last value");
+  return m_lastPosition;
+}
+
+void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) {
+  m_distancePerRotation = distancePerRotation;
+}
+
+double DutyCycleEncoder::GetDistancePerRotation() const {
+  return m_distancePerRotation;
+}
+
+double DutyCycleEncoder::GetDistance() const {
+  return Get().to<double>() * GetDistancePerRotation();
+}
+
+int DutyCycleEncoder::GetFrequency() const {
+  return m_dutyCycle->GetFrequency();
+}
+
+void DutyCycleEncoder::Reset() {
+  m_counter.Reset();
+  m_positionOffset = m_dutyCycle->GetOutput();
+}
+
+bool DutyCycleEncoder::IsConnected() const {
+  if (m_simIsConnected) return m_simIsConnected.Get();
+  return GetFrequency() > m_frequencyThreshold;
+}
+
+void DutyCycleEncoder::SetConnectedFrequencyThreshold(int frequency) {
+  if (frequency < 0) {
+    frequency = 0;
+  }
+  m_frequencyThreshold = frequency;
+}
+
+void DutyCycleEncoder::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("AbsoluteEncoder");
+  builder.AddDoubleProperty("Distance", [this] { return this->GetDistance(); },
+                            nullptr);
+  builder.AddDoubleProperty("Distance Per Rotation",
+                            [this] { return this->GetDistancePerRotation(); },
+                            nullptr);
+  builder.AddDoubleProperty("Is Connected",
+                            [this] { return this->IsConnected(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/Encoder.cpp b/wpilibc/src/main/native/cpp/Encoder.cpp
index fb043cc..963bcc6 100644
--- a/wpilibc/src/main/native/cpp/Encoder.cpp
+++ b/wpilibc/src/main/native/cpp/Encoder.cpp
@@ -11,7 +11,6 @@
 
 #include <hal/Encoder.h>
 #include <hal/FRCUsageReporting.h>
-#include <hal/HALBase.h>
 
 #include "frc/DigitalInput.h"
 #include "frc/WPIErrors.h"
@@ -60,14 +59,14 @@
 Encoder::~Encoder() {
   int32_t status = 0;
   HAL_FreeEncoder(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int Encoder::Get() const {
   if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetEncoder(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -75,14 +74,14 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_ResetEncoder(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 double Encoder::GetPeriod() const {
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetEncoderPeriod(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -90,14 +89,14 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 bool Encoder::GetStopped() const {
   if (StatusIsFatal()) return true;
   int32_t status = 0;
   bool value = HAL_GetEncoderStopped(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -105,7 +104,7 @@
   if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool value = HAL_GetEncoderDirection(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -113,14 +112,14 @@
   if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetEncoderRaw(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
 int Encoder::GetEncodingScale() const {
   int32_t status = 0;
   int val = HAL_GetEncoderEncodingScale(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return val;
 }
 
@@ -128,7 +127,7 @@
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetEncoderDistance(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -136,7 +135,7 @@
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetEncoderRate(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -144,21 +143,21 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetEncoderMinRate(m_encoder, minRate, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Encoder::SetDistancePerPulse(double distancePerPulse) {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetEncoderDistancePerPulse(m_encoder, distancePerPulse, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 double Encoder::GetDistancePerPulse() const {
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double distancePerPulse = HAL_GetEncoderDistancePerPulse(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return distancePerPulse;
 }
 
@@ -166,7 +165,7 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetEncoderReverseDirection(m_encoder, reverseDirection, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Encoder::SetSamplesToAverage(int samplesToAverage) {
@@ -178,13 +177,13 @@
   }
   int32_t status = 0;
   HAL_SetEncoderSamplesToAverage(m_encoder, samplesToAverage, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int Encoder::GetSamplesToAverage() const {
   int32_t status = 0;
   int result = HAL_GetEncoderSamplesToAverage(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return result;
 }
 
@@ -214,7 +213,7 @@
       m_encoder, source.GetPortHandleForRouting(),
       (HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(),
       (HAL_EncoderIndexingType)type, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Encoder::SetSimDevice(HAL_SimDeviceHandle device) {
@@ -224,14 +223,14 @@
 int Encoder::GetFPGAIndex() const {
   int32_t status = 0;
   int val = HAL_GetEncoderFPGAIndex(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return val;
 }
 
 void Encoder::InitSendable(SendableBuilder& builder) {
   int32_t status = 0;
   HAL_EncoderEncodingType type = HAL_GetEncoderEncodingType(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   if (type == HAL_EncoderEncodingType::HAL_Encoder_k4X)
     builder.SetSmartDashboardType("Quadrature Encoder");
   else
@@ -252,9 +251,9 @@
       m_bSource->GetPortHandleForRouting(),
       (HAL_AnalogTriggerType)m_bSource->GetAnalogTriggerTypeForRouting(),
       reverseDirection, (HAL_EncoderEncodingType)encodingType, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
-  HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex(),
+  HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex() + 1,
              encodingType);
   SendableRegistry::GetInstance().AddLW(this, "Encoder",
                                         m_aSource->GetChannel());
@@ -264,6 +263,6 @@
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double val = HAL_GetEncoderDecodingScaleFactor(m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return val;
 }
diff --git a/wpilibc/src/main/native/cpp/ErrorBase.cpp b/wpilibc/src/main/native/cpp/ErrorBase.cpp
index 8e70e61..d098c07 100644
--- a/wpilibc/src/main/native/cpp/ErrorBase.cpp
+++ b/wpilibc/src/main/native/cpp/ErrorBase.cpp
@@ -12,7 +12,8 @@
 #include <cstring>
 #include <set>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
 #include <wpi/Format.h>
 #include <wpi/SmallString.h>
 #include <wpi/raw_ostream.h>
diff --git a/wpilibc/src/main/native/cpp/GenericHID.cpp b/wpilibc/src/main/native/cpp/GenericHID.cpp
index 504d669..a7dcfc0 100644
--- a/wpilibc/src/main/native/cpp/GenericHID.cpp
+++ b/wpilibc/src/main/native/cpp/GenericHID.cpp
@@ -7,7 +7,8 @@
 
 #include "frc/GenericHID.h"
 
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/DriverStation.h"
 #include "frc/WPIErrors.h"
diff --git a/wpilibc/src/main/native/cpp/I2C.cpp b/wpilibc/src/main/native/cpp/I2C.cpp
index 44b71af..21d92f2 100644
--- a/wpilibc/src/main/native/cpp/I2C.cpp
+++ b/wpilibc/src/main/native/cpp/I2C.cpp
@@ -9,7 +9,7 @@
 
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <hal/I2C.h>
 
 #include "frc/WPIErrors.h"
@@ -20,7 +20,7 @@
     : m_port(static_cast<HAL_I2CPort>(port)), m_deviceAddress(deviceAddress) {
   int32_t status = 0;
   HAL_InitializeI2C(m_port, &status);
-  // wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  // wpi_setHALError(status);
 
   HAL_Report(HALUsageReporting::kResourceType_I2C, deviceAddress);
 }
@@ -32,7 +32,7 @@
   int32_t status = 0;
   status = HAL_TransactionI2C(m_port, m_deviceAddress, dataToSend, sendSize,
                               dataReceived, receiveSize);
-  // wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  // wpi_setHALError(status);
   return status < 0;
 }
 
diff --git a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
index 36150d4..202c61d 100644
--- a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
+++ b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/InterruptableSensorBase.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/Utility.h"
 #include "frc/WPIErrors.h"
@@ -36,7 +36,7 @@
       &status);
   SetUpSourceEdge(true, false);
   HAL_AttachInterruptHandler(m_interrupt, handler, param, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void InterruptableSensorBase::RequestInterrupts(InterruptEventHandler handler) {
@@ -69,7 +69,7 @@
         (*self)(res);
       },
       m_interruptHandler.get(), &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void InterruptableSensorBase::RequestInterrupts() {
@@ -84,7 +84,7 @@
       m_interrupt, GetPortHandleForRouting(),
       static_cast<HAL_AnalogTriggerType>(GetAnalogTriggerTypeForRouting()),
       &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   SetUpSourceEdge(true, false);
 }
 
@@ -106,7 +106,7 @@
   int result;
 
   result = HAL_WaitForInterrupt(m_interrupt, timeout, ignorePrevious, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   // Rising edge result is the interrupt bit set in the byte 0xFF
   // Falling edge result is the interrupt bit set in the byte 0xFF00
@@ -122,7 +122,7 @@
   wpi_assert(m_interrupt != HAL_kInvalidHandle);
   int32_t status = 0;
   HAL_EnableInterrupts(m_interrupt, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void InterruptableSensorBase::DisableInterrupts() {
@@ -130,7 +130,7 @@
   wpi_assert(m_interrupt != HAL_kInvalidHandle);
   int32_t status = 0;
   HAL_DisableInterrupts(m_interrupt, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 double InterruptableSensorBase::ReadRisingTimestamp() {
@@ -138,7 +138,7 @@
   wpi_assert(m_interrupt != HAL_kInvalidHandle);
   int32_t status = 0;
   int64_t timestamp = HAL_ReadInterruptRisingTimestamp(m_interrupt, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return timestamp * 1e-6;
 }
 
@@ -147,7 +147,7 @@
   wpi_assert(m_interrupt != HAL_kInvalidHandle);
   int32_t status = 0;
   int64_t timestamp = HAL_ReadInterruptFallingTimestamp(m_interrupt, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return timestamp * 1e-6;
 }
 
@@ -163,7 +163,7 @@
   if (m_interrupt != HAL_kInvalidHandle) {
     int32_t status = 0;
     HAL_SetInterruptUpSourceEdge(m_interrupt, risingEdge, fallingEdge, &status);
-    wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+    wpi_setHALError(status);
   }
 }
 
@@ -172,5 +172,5 @@
   // Expects the calling leaf class to allocate an interrupt index.
   int32_t status = 0;
   m_interrupt = HAL_InitializeInterrupts(watcher, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
diff --git a/wpilibc/src/main/native/cpp/IterativeRobot.cpp b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
index 950b239..c8664a5 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobot.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
@@ -7,7 +7,8 @@
 
 #include "frc/IterativeRobot.h"
 
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/DriverStation.h"
 
@@ -30,7 +31,13 @@
   while (true) {
     // Wait for driver station data so the loop doesn't hog the CPU
     DriverStation::GetInstance().WaitForData();
+    if (m_exit) break;
 
     LoopFunc();
   }
 }
+
+void IterativeRobot::EndCompetition() {
+  m_exit = true;
+  DriverStation::GetInstance().WakeupWaitForData();
+}
diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
index 90dc54b..2997234 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
@@ -9,14 +9,14 @@
 
 #include <cstdio>
 
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
 #include <wpi/Format.h>
 #include <wpi/SmallString.h>
 #include <wpi/raw_ostream.h>
 
 #include "frc/DriverStation.h"
 #include "frc/Timer.h"
-#include "frc/commands/Scheduler.h"
 #include "frc/livewindow/LiveWindow.h"
 #include "frc/shuffleboard/Shuffleboard.h"
 #include "frc/smartdashboard/SmartDashboard.h"
@@ -131,7 +131,6 @@
       TeleopInit();
       m_watchdog.AddEpoch("TeleopInit()");
       m_lastMode = Mode::kTeleop;
-      Scheduler::GetInstance()->SetEnabled(true);
     }
 
     HAL_ObserveUserProgramTeleop();
diff --git a/wpilibc/src/main/native/cpp/Jaguar.cpp b/wpilibc/src/main/native/cpp/Jaguar.cpp
index 487d489..c87e4d1 100644
--- a/wpilibc/src/main/native/cpp/Jaguar.cpp
+++ b/wpilibc/src/main/native/cpp/Jaguar.cpp
@@ -7,26 +7,18 @@
 
 #include "frc/Jaguar.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 Jaguar::Jaguar(int channel) : PWMSpeedController(channel) {
-  /* Input profile defined by Luminary Micro.
-   *
-   * Full reverse ranges from 0.671325ms to 0.6972211ms
-   * Proportional reverse ranges from 0.6972211ms to 1.4482078ms
-   * Neutral ranges from 1.4482078ms to 1.5517922ms
-   * Proportional forward ranges from 1.5517922ms to 2.3027789ms
-   * Full forward ranges from 2.3027789ms to 2.328675ms
-   */
-  SetBounds(2.31, 1.55, 1.507, 1.454, .697);
+  SetBounds(2.31, 1.55, 1.507, 1.454, 0.697);
   SetPeriodMultiplier(kPeriodMultiplier_1X);
   SetSpeed(0.0);
   SetZeroLatch();
 
-  HAL_Report(HALUsageReporting::kResourceType_Jaguar, GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_Jaguar, GetChannel() + 1);
   SendableRegistry::GetInstance().SetName(this, "Jaguar", GetChannel());
 }
diff --git a/wpilibc/src/main/native/cpp/Joystick.cpp b/wpilibc/src/main/native/cpp/Joystick.cpp
index 34afd0a..8d464cf 100644
--- a/wpilibc/src/main/native/cpp/Joystick.cpp
+++ b/wpilibc/src/main/native/cpp/Joystick.cpp
@@ -9,7 +9,7 @@
 
 #include <cmath>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <wpi/math>
 
 #include "frc/DriverStation.h"
@@ -24,7 +24,7 @@
   m_axes[Axis::kTwist] = kDefaultTwistChannel;
   m_axes[Axis::kThrottle] = kDefaultThrottleChannel;
 
-  HAL_Report(HALUsageReporting::kResourceType_Joystick, port);
+  HAL_Report(HALUsageReporting::kResourceType_Joystick, port + 1);
 }
 
 void Joystick::SetXChannel(int channel) { m_axes[Axis::kX] = channel; }
diff --git a/wpilibc/src/main/native/cpp/LinearFilter.cpp b/wpilibc/src/main/native/cpp/LinearFilter.cpp
deleted file mode 100644
index 95df127..0000000
--- a/wpilibc/src/main/native/cpp/LinearFilter.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/LinearFilter.h"
-
-#include <cassert>
-#include <cmath>
-
-#include <hal/HAL.h>
-
-using namespace frc;
-
-LinearFilter::LinearFilter(wpi::ArrayRef<double> ffGains,
-                           wpi::ArrayRef<double> fbGains)
-    : m_inputs(ffGains.size()),
-      m_outputs(fbGains.size()),
-      m_inputGains(ffGains),
-      m_outputGains(fbGains) {
-  static int instances = 0;
-  instances++;
-  HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
-}
-
-LinearFilter LinearFilter::SinglePoleIIR(double timeConstant,
-                                         units::second_t period) {
-  double gain = std::exp(-period.to<double>() / timeConstant);
-  return LinearFilter(1.0 - gain, -gain);
-}
-
-LinearFilter LinearFilter::HighPass(double timeConstant,
-                                    units::second_t period) {
-  double gain = std::exp(-period.to<double>() / timeConstant);
-  return LinearFilter({gain, -gain}, {-gain});
-}
-
-LinearFilter LinearFilter::MovingAverage(int taps) {
-  assert(taps > 0);
-
-  std::vector<double> gains(taps, 1.0 / taps);
-  return LinearFilter(gains, {});
-}
-
-void LinearFilter::Reset() {
-  m_inputs.reset();
-  m_outputs.reset();
-}
-
-double LinearFilter::Calculate(double input) {
-  double retVal = 0.0;
-
-  // Rotate the inputs
-  m_inputs.push_front(input);
-
-  // Calculate the new value
-  for (size_t i = 0; i < m_inputGains.size(); i++) {
-    retVal += m_inputs[i] * m_inputGains[i];
-  }
-  for (size_t i = 0; i < m_outputGains.size(); i++) {
-    retVal -= m_outputs[i] * m_outputGains[i];
-  }
-
-  // Rotate the outputs
-  m_outputs.push_front(retVal);
-
-  return retVal;
-}
diff --git a/wpilibc/src/main/native/cpp/NidecBrushless.cpp b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
index 90ce43e..5bce36e 100644
--- a/wpilibc/src/main/native/cpp/NidecBrushless.cpp
+++ b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/NidecBrushless.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
@@ -26,7 +26,7 @@
   m_dio.SetPWMRate(15625);
   m_dio.EnablePWM(0.5);
 
-  HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel);
+  HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel + 1);
   registry.AddLW(this, "Nidec Brushless", pwmChannel);
 }
 
diff --git a/wpilibc/src/main/native/cpp/Notifier.cpp b/wpilibc/src/main/native/cpp/Notifier.cpp
index a2f93f9..a7fa038 100644
--- a/wpilibc/src/main/native/cpp/Notifier.cpp
+++ b/wpilibc/src/main/native/cpp/Notifier.cpp
@@ -9,7 +9,9 @@
 
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/Notifier.h>
+#include <wpi/SmallString.h>
 
 #include "frc/Timer.h"
 #include "frc/Utility.h"
@@ -23,7 +25,7 @@
   m_handler = handler;
   int32_t status = 0;
   m_notifier = HAL_InitializeNotifier(&status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   m_thread = std::thread([=] {
     for (;;) {
@@ -57,7 +59,7 @@
   // atomically set handle to 0, then clean
   HAL_NotifierHandle handle = m_notifier.exchange(0);
   HAL_StopNotifier(handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   // Join the thread to ensure the handler has exited.
   if (m_thread.joinable()) m_thread.join();
@@ -90,6 +92,13 @@
   return *this;
 }
 
+void Notifier::SetName(const wpi::Twine& name) {
+  wpi::SmallString<64> nameBuf;
+  int32_t status = 0;
+  HAL_SetNotifierName(m_notifier,
+                      name.toNullTerminatedStringRef(nameBuf).data(), &status);
+}
+
 void Notifier::SetHandler(std::function<void()> handler) {
   std::scoped_lock lock(m_processMutex);
   m_handler = handler;
@@ -122,7 +131,7 @@
 void Notifier::Stop() {
   int32_t status = 0;
   HAL_CancelNotifierAlarm(m_notifier, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Notifier::UpdateAlarm(uint64_t triggerTime) {
@@ -131,7 +140,7 @@
   auto notifier = m_notifier.load();
   if (notifier == 0) return;
   HAL_UpdateNotifierAlarm(notifier, triggerTime, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Notifier::UpdateAlarm() {
diff --git a/wpilibc/src/main/native/cpp/PIDBase.cpp b/wpilibc/src/main/native/cpp/PIDBase.cpp
index a2efe0f..c8f6fed 100644
--- a/wpilibc/src/main/native/cpp/PIDBase.cpp
+++ b/wpilibc/src/main/native/cpp/PIDBase.cpp
@@ -10,7 +10,7 @@
 #include <algorithm>
 #include <cmath>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/PIDOutput.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -35,7 +35,7 @@
   m_F = Kf;
 
   m_pidInput = &source;
-  m_filter = LinearFilter::MovingAverage(1);
+  m_filter = LinearFilter<double>::MovingAverage(1);
 
   m_pidOutput = &output;
 
@@ -196,7 +196,7 @@
 
 void PIDBase::SetToleranceBuffer(int bufLength) {
   std::scoped_lock lock(m_thisMutex);
-  m_filter = LinearFilter::MovingAverage(bufLength);
+  m_filter = LinearFilter<double>::MovingAverage(bufLength);
 }
 
 bool PIDBase::OnTarget() const {
diff --git a/wpilibc/src/main/native/cpp/PWM.cpp b/wpilibc/src/main/native/cpp/PWM.cpp
index c6b398f..dabcd2e 100644
--- a/wpilibc/src/main/native/cpp/PWM.cpp
+++ b/wpilibc/src/main/native/cpp/PWM.cpp
@@ -9,7 +9,8 @@
 
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
 #include <hal/PWM.h>
 #include <hal/Ports.h>
 
@@ -31,8 +32,7 @@
   int32_t status = 0;
   m_handle = HAL_InitializePWMPort(HAL_GetPort(channel), &status);
   if (status != 0) {
-    wpi_setErrorWithContextRange(status, 0, HAL_GetNumPWMChannels(), channel,
-                                 HAL_GetErrorMessage(status));
+    wpi_setHALErrorWithRange(status, 0, HAL_GetNumPWMChannels(), channel);
     m_channel = std::numeric_limits<int>::max();
     m_handle = HAL_kInvalidHandle;
     return;
@@ -41,12 +41,12 @@
   m_channel = channel;
 
   HAL_SetPWMDisabled(m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   status = 0;
   HAL_SetPWMEliminateDeadband(m_handle, false, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
-  HAL_Report(HALUsageReporting::kResourceType_PWM, channel);
+  HAL_Report(HALUsageReporting::kResourceType_PWM, channel + 1);
   SendableRegistry::GetInstance().AddLW(this, "PWM", channel);
 
   SetSafetyEnabled(false);
@@ -56,10 +56,10 @@
   int32_t status = 0;
 
   HAL_SetPWMDisabled(m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   HAL_FreePWMPort(m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void PWM::StopMotor() { SetDisabled(); }
@@ -73,7 +73,7 @@
 
   int32_t status = 0;
   HAL_SetPWMRaw(m_handle, value, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 uint16_t PWM::GetRaw() const {
@@ -81,7 +81,7 @@
 
   int32_t status = 0;
   uint16_t value = HAL_GetPWMRaw(m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   return value;
 }
@@ -90,14 +90,14 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetPWMPosition(m_handle, pos, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 double PWM::GetPosition() const {
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double position = HAL_GetPWMPosition(m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return position;
 }
 
@@ -105,7 +105,7 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetPWMSpeed(m_handle, speed, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   Feed();
 }
@@ -114,7 +114,7 @@
   if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double speed = HAL_GetPWMSpeed(m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return speed;
 }
 
@@ -124,7 +124,7 @@
   int32_t status = 0;
 
   HAL_SetPWMDisabled(m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void PWM::SetPeriodMultiplier(PeriodMultiplier mult) {
@@ -148,7 +148,7 @@
       wpi_assert(false);
   }
 
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void PWM::SetZeroLatch() {
@@ -157,14 +157,14 @@
   int32_t status = 0;
 
   HAL_LatchPWMZero(m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void PWM::EnableDeadbandElimination(bool eliminateDeadband) {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetPWMEliminateDeadband(m_handle, eliminateDeadband, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void PWM::SetBounds(double max, double deadbandMax, double center,
@@ -173,7 +173,7 @@
   int32_t status = 0;
   HAL_SetPWMConfig(m_handle, max, deadbandMax, center, deadbandMin, min,
                    &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void PWM::SetRawBounds(int max, int deadbandMax, int center, int deadbandMin,
@@ -182,7 +182,7 @@
   int32_t status = 0;
   HAL_SetPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min,
                       &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void PWM::GetRawBounds(int* max, int* deadbandMax, int* center,
@@ -190,7 +190,7 @@
   int32_t status = 0;
   HAL_GetPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min,
                       &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int PWM::GetChannel() const { return m_channel; }
diff --git a/wpilibc/src/main/native/cpp/PWMSparkMax.cpp b/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
index d8d3b2a..c5375f4 100644
--- a/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
+++ b/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
@@ -7,26 +7,18 @@
 
 #include "frc/PWMSparkMax.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 PWMSparkMax::PWMSparkMax(int channel) : PWMSpeedController(channel) {
-  /* Note that the SparkMax uses the following bounds for PWM values.
-   *
-   *   2.003ms = full "forward"
-   *   1.55ms = the "high end" of the deadband range
-   *   1.50ms = center of the deadband range (off)
-   *   1.46ms = the "low end" of the deadband range
-   *   0.999ms = full "reverse"
-   */
-  SetBounds(2.003, 1.55, 1.50, 1.46, .999);
+  SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
   SetPeriodMultiplier(kPeriodMultiplier_1X);
   SetSpeed(0.0);
   SetZeroLatch();
 
-  HAL_Report(HALUsageReporting::kResourceType_RevSparkMaxPWM, GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_RevSparkMaxPWM, GetChannel() + 1);
   SendableRegistry::GetInstance().SetName(this, "PWMSparkMax", GetChannel());
 }
diff --git a/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
index 0bbc5b9..1242be9 100644
--- a/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
+++ b/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
@@ -7,30 +7,18 @@
 
 #include "frc/PWMTalonSRX.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 PWMTalonSRX::PWMTalonSRX(int channel) : PWMSpeedController(channel) {
-  /* Note that the PWMTalonSRX uses the following bounds for PWM values. These
-   * values should work reasonably well for most controllers, but if users
-   * experience issues such as asymmetric behavior around the deadband or
-   * inability to saturate the controller in either direction, calibration is
-   * recommended. The calibration procedure can be found in the TalonSRX User
-   * Manual available from Cross The Road Electronics.
-   *   2.004ms = full "forward"
-   *   1.52ms = the "high end" of the deadband range
-   *   1.50ms = center of the deadband range (off)
-   *   1.48ms = the "low end" of the deadband range
-   *   0.997ms = full "reverse"
-   */
-  SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
   SetPeriodMultiplier(kPeriodMultiplier_1X);
   SetSpeed(0.0);
   SetZeroLatch();
 
-  HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel() + 1);
   SendableRegistry::GetInstance().SetName(this, "PWMTalonSRX", GetChannel());
 }
diff --git a/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp b/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
index 122d219..0d966ae 100644
--- a/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
+++ b/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
@@ -7,30 +7,18 @@
 
 #include "frc/PWMVictorSPX.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 PWMVictorSPX::PWMVictorSPX(int channel) : PWMSpeedController(channel) {
-  /* Note that the PWMVictorSPX uses the following bounds for PWM values. These
-   * values should work reasonably well for most controllers, but if users
-   * experience issues such as asymmetric behavior around the deadband or
-   * inability to saturate the controller in either direction, calibration is
-   * recommended. The calibration procedure can be found in the VictorSPX User
-   * Manual available from Cross The Road Electronics.
-   *   2.004ms = full "forward"
-   *   1.52ms = the "high end" of the deadband range
-   *   1.50ms = center of the deadband range (off)
-   *   1.48ms = the "low end" of the deadband range
-   *   0.997ms = full "reverse"
-   */
-  SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
   SetPeriodMultiplier(kPeriodMultiplier_1X);
   SetSpeed(0.0);
   SetZeroLatch();
 
-  HAL_Report(HALUsageReporting::kResourceType_PWMVictorSPX, GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_PWMVictorSPX, GetChannel() + 1);
   SendableRegistry::GetInstance().SetName(this, "PWMVictorSPX", GetChannel());
 }
diff --git a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
index b3297ea..7d7d916 100644
--- a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
+++ b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/PowerDistributionPanel.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <hal/PDP.h>
 #include <hal/Ports.h>
 #include <wpi/SmallString.h>
@@ -29,12 +29,11 @@
   int32_t status = 0;
   m_handle = HAL_InitializePDP(module, &status);
   if (status != 0) {
-    wpi_setErrorWithContextRange(status, 0, HAL_GetNumPDPModules(), module,
-                                 HAL_GetErrorMessage(status));
+    wpi_setHALErrorWithRange(status, 0, HAL_GetNumPDPModules(), module);
     return;
   }
 
-  HAL_Report(HALUsageReporting::kResourceType_PDP, module);
+  HAL_Report(HALUsageReporting::kResourceType_PDP, module + 1);
   SendableRegistry::GetInstance().AddLW(this, "PowerDistributionPanel", module);
 }
 
diff --git a/wpilibc/src/main/native/cpp/Preferences.cpp b/wpilibc/src/main/native/cpp/Preferences.cpp
index 7a896d1..6014775 100644
--- a/wpilibc/src/main/native/cpp/Preferences.cpp
+++ b/wpilibc/src/main/native/cpp/Preferences.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -9,7 +9,7 @@
 
 #include <algorithm>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <networktables/NetworkTableInstance.h>
 #include <wpi/StringRef.h>
 
diff --git a/wpilibc/src/main/native/cpp/Relay.cpp b/wpilibc/src/main/native/cpp/Relay.cpp
index 3a3e772..cb95223 100644
--- a/wpilibc/src/main/native/cpp/Relay.cpp
+++ b/wpilibc/src/main/native/cpp/Relay.cpp
@@ -9,7 +9,8 @@
 
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
 #include <hal/Ports.h>
 #include <hal/Relay.h>
 #include <wpi/raw_ostream.h>
@@ -35,20 +36,18 @@
     int32_t status = 0;
     m_forwardHandle = HAL_InitializeRelayPort(portHandle, true, &status);
     if (status != 0) {
-      wpi_setErrorWithContextRange(status, 0, HAL_GetNumRelayChannels(),
-                                   channel, HAL_GetErrorMessage(status));
+      wpi_setHALErrorWithRange(status, 0, HAL_GetNumRelayChannels(), channel);
       m_forwardHandle = HAL_kInvalidHandle;
       m_reverseHandle = HAL_kInvalidHandle;
       return;
     }
-    HAL_Report(HALUsageReporting::kResourceType_Relay, m_channel);
+    HAL_Report(HALUsageReporting::kResourceType_Relay, m_channel + 1);
   }
   if (m_direction == kBothDirections || m_direction == kReverseOnly) {
     int32_t status = 0;
     m_reverseHandle = HAL_InitializeRelayPort(portHandle, false, &status);
     if (status != 0) {
-      wpi_setErrorWithContextRange(status, 0, HAL_GetNumRelayChannels(),
-                                   channel, HAL_GetErrorMessage(status));
+      wpi_setHALErrorWithRange(status, 0, HAL_GetNumRelayChannels(), channel);
       m_forwardHandle = HAL_kInvalidHandle;
       m_reverseHandle = HAL_kInvalidHandle;
       return;
@@ -61,7 +60,7 @@
   if (m_forwardHandle != HAL_kInvalidHandle) {
     HAL_SetRelay(m_forwardHandle, false, &status);
     if (status != 0) {
-      wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+      wpi_setHALError(status);
       m_forwardHandle = HAL_kInvalidHandle;
       m_reverseHandle = HAL_kInvalidHandle;
       return;
@@ -70,7 +69,7 @@
   if (m_reverseHandle != HAL_kInvalidHandle) {
     HAL_SetRelay(m_reverseHandle, false, &status);
     if (status != 0) {
-      wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+      wpi_setHALError(status);
       m_forwardHandle = HAL_kInvalidHandle;
       m_reverseHandle = HAL_kInvalidHandle;
       return;
@@ -137,7 +136,7 @@
       break;
   }
 
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 Relay::Value Relay::Get() const {
@@ -171,7 +170,7 @@
     }
   }
 
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int Relay::GetChannel() const { return m_channel; }
diff --git a/wpilibc/src/main/native/cpp/RobotController.cpp b/wpilibc/src/main/native/cpp/RobotController.cpp
index 347440d..b1d7608 100644
--- a/wpilibc/src/main/native/cpp/RobotController.cpp
+++ b/wpilibc/src/main/native/cpp/RobotController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,10 @@
 
 #include "frc/RobotController.h"
 
-#include <hal/HAL.h>
+#include <hal/CAN.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
+#include <hal/Power.h>
 
 #include "frc/ErrorBase.h"
 
@@ -16,21 +19,21 @@
 int RobotController::GetFPGAVersion() {
   int32_t status = 0;
   int version = HAL_GetFPGAVersion(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return version;
 }
 
 int64_t RobotController::GetFPGARevision() {
   int32_t status = 0;
   int64_t revision = HAL_GetFPGARevision(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return revision;
 }
 
 uint64_t RobotController::GetFPGATime() {
   int32_t status = 0;
   uint64_t time = HAL_GetFPGATime(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return time;
 }
 
@@ -46,112 +49,112 @@
 bool RobotController::IsSysActive() {
   int32_t status = 0;
   bool retVal = HAL_GetSystemActive(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 bool RobotController::IsBrownedOut() {
   int32_t status = 0;
   bool retVal = HAL_GetBrownedOut(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 double RobotController::GetInputVoltage() {
   int32_t status = 0;
   double retVal = HAL_GetVinVoltage(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 double RobotController::GetInputCurrent() {
   int32_t status = 0;
   double retVal = HAL_GetVinCurrent(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 double RobotController::GetVoltage3V3() {
   int32_t status = 0;
   double retVal = HAL_GetUserVoltage3V3(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 double RobotController::GetCurrent3V3() {
   int32_t status = 0;
   double retVal = HAL_GetUserCurrent3V3(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 bool RobotController::GetEnabled3V3() {
   int32_t status = 0;
   bool retVal = HAL_GetUserActive3V3(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 int RobotController::GetFaultCount3V3() {
   int32_t status = 0;
   int retVal = HAL_GetUserCurrentFaults3V3(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 double RobotController::GetVoltage5V() {
   int32_t status = 0;
   double retVal = HAL_GetUserVoltage5V(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 double RobotController::GetCurrent5V() {
   int32_t status = 0;
   double retVal = HAL_GetUserCurrent5V(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 bool RobotController::GetEnabled5V() {
   int32_t status = 0;
   bool retVal = HAL_GetUserActive5V(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 int RobotController::GetFaultCount5V() {
   int32_t status = 0;
   int retVal = HAL_GetUserCurrentFaults5V(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 double RobotController::GetVoltage6V() {
   int32_t status = 0;
   double retVal = HAL_GetUserVoltage6V(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 double RobotController::GetCurrent6V() {
   int32_t status = 0;
   double retVal = HAL_GetUserCurrent6V(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 bool RobotController::GetEnabled6V() {
   int32_t status = 0;
   bool retVal = HAL_GetUserActive6V(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
 int RobotController::GetFaultCount6V() {
   int32_t status = 0;
   int retVal = HAL_GetUserCurrentFaults6V(&status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return retVal;
 }
 
@@ -165,7 +168,7 @@
   HAL_CAN_GetCANStatus(&percentBusUtilization, &busOffCount, &txFullCount,
                        &receiveErrorCount, &transmitErrorCount, &status);
   if (status != 0) {
-    wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+    wpi_setGlobalHALError(status);
     return {};
   }
   return {percentBusUtilization, static_cast<int>(busOffCount),
diff --git a/wpilibc/src/main/native/cpp/RobotDrive.cpp b/wpilibc/src/main/native/cpp/RobotDrive.cpp
index fd43c96..5235fa2 100644
--- a/wpilibc/src/main/native/cpp/RobotDrive.cpp
+++ b/wpilibc/src/main/native/cpp/RobotDrive.cpp
@@ -10,7 +10,7 @@
 #include <algorithm>
 #include <cmath>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/GenericHID.h"
 #include "frc/Joystick.h"
@@ -128,13 +128,13 @@
   if (curve < 0) {
     double value = std::log(-curve);
     double ratio = (value - m_sensitivity) / (value + m_sensitivity);
-    if (ratio == 0) ratio = .0000000001;
+    if (ratio == 0) ratio = 0.0000000001;
     leftOutput = outputMagnitude / ratio;
     rightOutput = outputMagnitude;
   } else if (curve > 0) {
     double value = std::log(curve);
     double ratio = (value - m_sensitivity) / (value + m_sensitivity);
-    if (ratio == 0) ratio = .0000000001;
+    if (ratio == 0) ratio = 0.0000000001;
     leftOutput = outputMagnitude;
     rightOutput = outputMagnitude / ratio;
   } else {
diff --git a/wpilibc/src/main/native/cpp/SD540.cpp b/wpilibc/src/main/native/cpp/SD540.cpp
index 93733b2..9611f66 100644
--- a/wpilibc/src/main/native/cpp/SD540.cpp
+++ b/wpilibc/src/main/native/cpp/SD540.cpp
@@ -7,31 +7,19 @@
 
 #include "frc/SD540.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 SD540::SD540(int channel) : PWMSpeedController(channel) {
-  /* Note that the SD540 uses the following bounds for PWM values. These values
-   * should work reasonably well for most controllers, but if users experience
-   * issues such as asymmetric behavior around the deadband or inability to
-   * saturate the controller in either direction, calibration is recommended.
-   * The calibration procedure can be found in the SD540 User Manual available
-   * from Mindsensors.
-   *
-   *   2.05ms = full "forward"
-   *   1.55ms = the "high end" of the deadband range
-   *   1.50ms = center of the deadband range (off)
-   *   1.44ms = the "low end" of the deadband range
-   *   0.94ms = full "reverse"
-   */
-  SetBounds(2.05, 1.55, 1.50, 1.44, .94);
+  SetBounds(2.05, 1.55, 1.50, 1.44, 0.94);
   SetPeriodMultiplier(kPeriodMultiplier_1X);
   SetSpeed(0.0);
   SetZeroLatch();
 
-  HAL_Report(HALUsageReporting::kResourceType_MindsensorsSD540, GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_MindsensorsSD540,
+             GetChannel() + 1);
   SendableRegistry::GetInstance().SetName(this, "SD540", GetChannel());
 }
diff --git a/wpilibc/src/main/native/cpp/SPI.cpp b/wpilibc/src/main/native/cpp/SPI.cpp
index 54cf82b..074cc79 100644
--- a/wpilibc/src/main/native/cpp/SPI.cpp
+++ b/wpilibc/src/main/native/cpp/SPI.cpp
@@ -10,7 +10,7 @@
 #include <cstring>
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <hal/SPI.h>
 #include <wpi/SmallVector.h>
 #include <wpi/mutex.h>
@@ -155,9 +155,10 @@
 SPI::SPI(Port port) : m_port(static_cast<HAL_SPIPort>(port)) {
   int32_t status = 0;
   HAL_InitializeSPI(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
-  HAL_Report(HALUsageReporting::kResourceType_SPI, port);
+  HAL_Report(HALUsageReporting::kResourceType_SPI,
+             static_cast<uint8_t>(port) + 1);
 }
 
 SPI::~SPI() { HAL_CloseSPI(m_port); }
@@ -207,13 +208,13 @@
 void SPI::SetChipSelectActiveHigh() {
   int32_t status = 0;
   HAL_SetSPIChipSelectActiveHigh(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SPI::SetChipSelectActiveLow() {
   int32_t status = 0;
   HAL_SetSPIChipSelectActiveLow(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int SPI::Write(uint8_t* data, int size) {
@@ -243,26 +244,26 @@
 void SPI::InitAuto(int bufferSize) {
   int32_t status = 0;
   HAL_InitSPIAuto(m_port, bufferSize, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SPI::FreeAuto() {
   int32_t status = 0;
   HAL_FreeSPIAuto(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SPI::SetAutoTransmitData(wpi::ArrayRef<uint8_t> dataToSend, int zeroSize) {
   int32_t status = 0;
   HAL_SetSPIAutoTransmitData(m_port, dataToSend.data(), dataToSend.size(),
                              zeroSize, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SPI::StartAutoRate(units::second_t period) {
   int32_t status = 0;
   HAL_StartSPIAutoRate(m_port, period.to<double>(), &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SPI::StartAutoRate(double period) {
@@ -275,19 +276,19 @@
       m_port, source.GetPortHandleForRouting(),
       (HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(), rising,
       falling, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SPI::StopAuto() {
   int32_t status = 0;
   HAL_StopSPIAuto(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SPI::ForceAutoRead() {
   int32_t status = 0;
   HAL_ForceSPIAutoRead(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead,
@@ -295,7 +296,7 @@
   int32_t status = 0;
   int32_t val = HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead,
                                             timeout.to<double>(), &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return val;
 }
 
@@ -306,7 +307,7 @@
 int SPI::GetAutoDroppedCount() {
   int32_t status = 0;
   int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return val;
 }
 
diff --git a/wpilibc/src/main/native/cpp/SensorUtil.cpp b/wpilibc/src/main/native/cpp/SensorUtil.cpp
index 2012da4..8c02f1b 100644
--- a/wpilibc/src/main/native/cpp/SensorUtil.cpp
+++ b/wpilibc/src/main/native/cpp/SensorUtil.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -10,7 +10,7 @@
 #include <hal/AnalogInput.h>
 #include <hal/AnalogOutput.h>
 #include <hal/DIO.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <hal/PDP.h>
 #include <hal/PWM.h>
 #include <hal/Ports.h>
diff --git a/wpilibc/src/main/native/cpp/SerialPort.cpp b/wpilibc/src/main/native/cpp/SerialPort.cpp
index 46e02c8..e092fc2 100644
--- a/wpilibc/src/main/native/cpp/SerialPort.cpp
+++ b/wpilibc/src/main/native/cpp/SerialPort.cpp
@@ -9,7 +9,7 @@
 
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <hal/SerialPort.h>
 
 using namespace frc;
@@ -21,17 +21,17 @@
 
   m_portHandle =
       HAL_InitializeSerialPort(static_cast<HAL_SerialPort>(port), &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   // Don't continue if initialization failed
   if (status < 0) return;
   HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   HAL_SetSerialParity(m_portHandle, parity, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   // Set the default timeout to 5 seconds.
   SetTimeout(5.0);
@@ -41,7 +41,8 @@
 
   DisableTermination();
 
-  HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
+  HAL_Report(HALUsageReporting::kResourceType_SerialPort,
+             static_cast<uint8_t>(port) + 1);
 }
 
 SerialPort::SerialPort(int baudRate, const wpi::Twine& portName, Port port,
@@ -54,17 +55,17 @@
 
   m_portHandle = HAL_InitializeSerialPortDirect(
       static_cast<HAL_SerialPort>(port), portNameC, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   // Don't continue if initialization failed
   if (status < 0) return;
   HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   HAL_SetSerialParity(m_portHandle, parity, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   // Set the default timeout to 5 seconds.
   SetTimeout(5.0);
@@ -74,44 +75,45 @@
 
   DisableTermination();
 
-  HAL_Report(HALUsageReporting::kResourceType_SerialPort, 0);
+  HAL_Report(HALUsageReporting::kResourceType_SerialPort,
+             static_cast<uint8_t>(port) + 1);
 }
 
 SerialPort::~SerialPort() {
   int32_t status = 0;
   HAL_CloseSerial(m_portHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl) {
   int32_t status = 0;
   HAL_SetSerialFlowControl(m_portHandle, flowControl, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SerialPort::EnableTermination(char terminator) {
   int32_t status = 0;
   HAL_EnableSerialTermination(m_portHandle, terminator, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SerialPort::DisableTermination() {
   int32_t status = 0;
   HAL_DisableSerialTermination(m_portHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 int SerialPort::GetBytesReceived() {
   int32_t status = 0;
   int retVal = HAL_GetSerialBytesReceived(m_portHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return retVal;
 }
 
 int SerialPort::Read(char* buffer, int count) {
   int32_t status = 0;
   int retVal = HAL_ReadSerial(m_portHandle, buffer, count, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return retVal;
 }
 
@@ -123,42 +125,42 @@
   int32_t status = 0;
   int retVal =
       HAL_WriteSerial(m_portHandle, buffer.data(), buffer.size(), &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return retVal;
 }
 
 void SerialPort::SetTimeout(double timeout) {
   int32_t status = 0;
   HAL_SetSerialTimeout(m_portHandle, timeout, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SerialPort::SetReadBufferSize(int size) {
   int32_t status = 0;
   HAL_SetSerialReadBufferSize(m_portHandle, size, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SerialPort::SetWriteBufferSize(int size) {
   int32_t status = 0;
   HAL_SetSerialWriteBufferSize(m_portHandle, size, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SerialPort::SetWriteBufferMode(SerialPort::WriteBufferMode mode) {
   int32_t status = 0;
   HAL_SetSerialWriteMode(m_portHandle, mode, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SerialPort::Flush() {
   int32_t status = 0;
   HAL_FlushSerial(m_portHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void SerialPort::Reset() {
   int32_t status = 0;
   HAL_ClearSerial(m_portHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
diff --git a/wpilibc/src/main/native/cpp/Servo.cpp b/wpilibc/src/main/native/cpp/Servo.cpp
index 09e482b..4b6856a 100644
--- a/wpilibc/src/main/native/cpp/Servo.cpp
+++ b/wpilibc/src/main/native/cpp/Servo.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/Servo.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
@@ -27,7 +27,7 @@
   // Assign defaults for period multiplier for the servo PWM control signal
   SetPeriodMultiplier(kPeriodMultiplier_4X);
 
-  HAL_Report(HALUsageReporting::kResourceType_Servo, channel);
+  HAL_Report(HALUsageReporting::kResourceType_Servo, channel + 1);
   SendableRegistry::GetInstance().SetName(this, "Servo", channel);
 }
 
diff --git a/wpilibc/src/main/native/cpp/Solenoid.cpp b/wpilibc/src/main/native/cpp/Solenoid.cpp
index c860c44..e0bde3c 100644
--- a/wpilibc/src/main/native/cpp/Solenoid.cpp
+++ b/wpilibc/src/main/native/cpp/Solenoid.cpp
@@ -9,7 +9,8 @@
 
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
 #include <hal/Ports.h>
 #include <hal/Solenoid.h>
 
@@ -40,14 +41,13 @@
   m_solenoidHandle = HAL_InitializeSolenoidPort(
       HAL_GetPortWithModule(moduleNumber, channel), &status);
   if (status != 0) {
-    wpi_setErrorWithContextRange(status, 0, HAL_GetNumSolenoidChannels(),
-                                 channel, HAL_GetErrorMessage(status));
+    wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(), channel);
     m_solenoidHandle = HAL_kInvalidHandle;
     return;
   }
 
-  HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel,
-             m_moduleNumber);
+  HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel + 1,
+             m_moduleNumber + 1);
   SendableRegistry::GetInstance().AddLW(this, "Solenoid", m_moduleNumber,
                                         m_channel);
 }
@@ -58,14 +58,14 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetSolenoid(m_solenoidHandle, on, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 bool Solenoid::Get() const {
   if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool value = HAL_GetSolenoid(m_solenoidHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
   return value;
 }
 
@@ -79,14 +79,14 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetOneShotDuration(m_solenoidHandle, durationMS, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Solenoid::StartPulse() {
   if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_FireOneShot(m_solenoidHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
 
 void Solenoid::InitSendable(SendableBuilder& builder) {
diff --git a/wpilibc/src/main/native/cpp/SolenoidBase.cpp b/wpilibc/src/main/native/cpp/SolenoidBase.cpp
index c0b79a5..f5f8aad 100644
--- a/wpilibc/src/main/native/cpp/SolenoidBase.cpp
+++ b/wpilibc/src/main/native/cpp/SolenoidBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,7 +7,7 @@
 
 #include "frc/SolenoidBase.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <hal/Solenoid.h>
 
 using namespace frc;
@@ -16,7 +16,7 @@
   int value = 0;
   int32_t status = 0;
   value = HAL_GetAllSolenoids(module, &status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return value;
 }
 
diff --git a/wpilibc/src/main/native/cpp/Spark.cpp b/wpilibc/src/main/native/cpp/Spark.cpp
index 18f8ee6..3717df4 100644
--- a/wpilibc/src/main/native/cpp/Spark.cpp
+++ b/wpilibc/src/main/native/cpp/Spark.cpp
@@ -7,31 +7,18 @@
 
 #include "frc/Spark.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 Spark::Spark(int channel) : PWMSpeedController(channel) {
-  /* Note that the Spark uses the following bounds for PWM values. These values
-   * should work reasonably well for most controllers, but if users experience
-   * issues such as asymmetric behavior around the deadband or inability to
-   * saturate the controller in either direction, calibration is recommended.
-   * The calibration procedure can be found in the Spark User Manual available
-   * from REV Robotics.
-   *
-   *   2.003ms = full "forward"
-   *   1.55ms = the "high end" of the deadband range
-   *   1.50ms = center of the deadband range (off)
-   *   1.46ms = the "low end" of the deadband range
-   *   0.999ms = full "reverse"
-   */
-  SetBounds(2.003, 1.55, 1.50, 1.46, .999);
+  SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
   SetPeriodMultiplier(kPeriodMultiplier_1X);
   SetSpeed(0.0);
   SetZeroLatch();
 
-  HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel() + 1);
   SendableRegistry::GetInstance().SetName(this, "Spark", GetChannel());
 }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp b/wpilibc/src/main/native/cpp/SpeedController.cpp
similarity index 65%
rename from wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
rename to wpilibc/src/main/native/cpp/SpeedController.cpp
index 8bd62ea..cb3cc5b 100644
--- a/wpilibc/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/wpilibc/src/main/native/cpp/SpeedController.cpp
@@ -5,12 +5,12 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include "frc2/command/PrintCommand.h"
+#include "frc/SpeedController.h"
 
-using namespace frc2;
+#include <frc/RobotController.h>
 
-PrintCommand::PrintCommand(const wpi::Twine& message)
-    : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
+using namespace frc;
+
+void SpeedController::SetVoltage(units::volt_t output) {
+  Set(output / units::volt_t(RobotController::GetInputVoltage()));
 }
-
-bool PrintCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibc/src/main/native/cpp/Talon.cpp b/wpilibc/src/main/native/cpp/Talon.cpp
index 8dc1928..4807d71 100644
--- a/wpilibc/src/main/native/cpp/Talon.cpp
+++ b/wpilibc/src/main/native/cpp/Talon.cpp
@@ -7,31 +7,18 @@
 
 #include "frc/Talon.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 Talon::Talon(int channel) : PWMSpeedController(channel) {
-  /* Note that the Talon uses the following bounds for PWM values. These values
-   * should work reasonably well for most controllers, but if users experience
-   * issues such as asymmetric behavior around the deadband or inability to
-   * saturate the controller in either direction, calibration is recommended.
-   * The calibration procedure can be found in the Talon User Manual available
-   * from CTRE.
-   *
-   *   2.037ms = full "forward"
-   *   1.539ms = the "high end" of the deadband range
-   *   1.513ms = center of the deadband range (off)
-   *   1.487ms = the "low end" of the deadband range
-   *   0.989ms = full "reverse"
-   */
-  SetBounds(2.037, 1.539, 1.513, 1.487, .989);
+  SetBounds(2.037, 1.539, 1.513, 1.487, 0.989);
   SetPeriodMultiplier(kPeriodMultiplier_1X);
   SetSpeed(0.0);
   SetZeroLatch();
 
-  HAL_Report(HALUsageReporting::kResourceType_Talon, GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_Talon, GetChannel() + 1);
   SendableRegistry::GetInstance().SetName(this, "Talon", GetChannel());
 }
diff --git a/wpilibc/src/main/native/cpp/Threads.cpp b/wpilibc/src/main/native/cpp/Threads.cpp
index 798e86a..7b1e647 100644
--- a/wpilibc/src/main/native/cpp/Threads.cpp
+++ b/wpilibc/src/main/native/cpp/Threads.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/Threads.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <hal/Threads.h>
 
 #include "frc/ErrorBase.h"
@@ -19,7 +19,7 @@
   HAL_Bool rt = false;
   auto native = thread.native_handle();
   auto ret = HAL_GetThreadPriority(&native, &rt, &status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   *isRealTime = rt;
   return ret;
 }
@@ -28,7 +28,7 @@
   int32_t status = 0;
   HAL_Bool rt = false;
   auto ret = HAL_GetCurrentThreadPriority(&rt, &status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   *isRealTime = rt;
   return ret;
 }
@@ -37,14 +37,14 @@
   int32_t status = 0;
   auto native = thread.native_handle();
   auto ret = HAL_SetThreadPriority(&native, realTime, priority, &status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return ret;
 }
 
 bool SetCurrentThreadPriority(bool realTime, int priority) {
   int32_t status = 0;
   auto ret = HAL_SetCurrentThreadPriority(realTime, priority, &status);
-  wpi_setGlobalErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setGlobalHALError(status);
   return ret;
 }
 
diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp
index ffff2dd..a9358dd 100644
--- a/wpilibc/src/main/native/cpp/TimedRobot.cpp
+++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp
@@ -11,7 +11,9 @@
 
 #include <utility>
 
-#include <hal/HAL.h>
+#include <hal/DriverStation.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/Notifier.h>
 
 #include "frc/Timer.h"
 #include "frc/Utility.h"
@@ -43,6 +45,11 @@
   }
 }
 
+void TimedRobot::EndCompetition() {
+  int32_t status = 0;
+  HAL_StopNotifier(m_notifier, &status);
+}
+
 units::second_t TimedRobot::GetPeriod() const {
   return units::second_t(m_period);
 }
@@ -52,7 +59,8 @@
 TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
   int32_t status = 0;
   m_notifier = HAL_InitializeNotifier(&status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
+  HAL_SetNotifierName(m_notifier, "TimedRobot", &status);
 
   HAL_Report(HALUsageReporting::kResourceType_Framework,
              HALUsageReporting::kFramework_Timed);
@@ -62,7 +70,7 @@
   int32_t status = 0;
 
   HAL_StopNotifier(m_notifier, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 
   HAL_CleanNotifier(m_notifier, &status);
 }
@@ -71,5 +79,5 @@
   int32_t status = 0;
   HAL_UpdateNotifierAlarm(
       m_notifier, static_cast<uint64_t>(m_expirationTime * 1e6), &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  wpi_setHALError(status);
 }
diff --git a/wpilibc/src/main/native/cpp/Timer.cpp b/wpilibc/src/main/native/cpp/Timer.cpp
index cdfa9ab..c91bc13 100644
--- a/wpilibc/src/main/native/cpp/Timer.cpp
+++ b/wpilibc/src/main/native/cpp/Timer.cpp
@@ -10,7 +10,7 @@
 #include <chrono>
 #include <thread>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/DriverStation.h"
 #include "frc/RobotController.h"
diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
index 35320de..fcd016e 100644
--- a/wpilibc/src/main/native/cpp/Ultrasonic.cpp
+++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/Ultrasonic.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/Counter.h"
 #include "frc/DigitalInput.h"
diff --git a/wpilibc/src/main/native/cpp/Utility.cpp b/wpilibc/src/main/native/cpp/Utility.cpp
index 499f9b4..11f6234 100644
--- a/wpilibc/src/main/native/cpp/Utility.cpp
+++ b/wpilibc/src/main/native/cpp/Utility.cpp
@@ -17,7 +17,7 @@
 #include <cstring>
 
 #include <hal/DriverStation.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <wpi/Path.h>
 #include <wpi/SmallString.h>
 #include <wpi/StackTrace.h>
diff --git a/wpilibc/src/main/native/cpp/Victor.cpp b/wpilibc/src/main/native/cpp/Victor.cpp
index 49dcd57..bce1913 100644
--- a/wpilibc/src/main/native/cpp/Victor.cpp
+++ b/wpilibc/src/main/native/cpp/Victor.cpp
@@ -7,32 +7,18 @@
 
 #include "frc/Victor.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 Victor::Victor(int channel) : PWMSpeedController(channel) {
-  /* Note that the Victor uses the following bounds for PWM values.  These
-   * values were determined empirically and optimized for the Victor 888. These
-   * values should work reasonably well for Victor 884 controllers as well but
-   * if users experience issues such as asymmetric behaviour around the deadband
-   * or inability to saturate the controller in either direction, calibration is
-   * recommended. The calibration procedure can be found in the Victor 884 User
-   * Manual available from IFI.
-   *
-   *   2.027ms = full "forward"
-   *   1.525ms = the "high end" of the deadband range
-   *   1.507ms = center of the deadband range (off)
-   *   1.49ms = the "low end" of the deadband range
-   *   1.026ms = full "reverse"
-   */
   SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
   SetPeriodMultiplier(kPeriodMultiplier_2X);
   SetSpeed(0.0);
   SetZeroLatch();
 
-  HAL_Report(HALUsageReporting::kResourceType_Victor, GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_Victor, GetChannel() + 1);
   SendableRegistry::GetInstance().SetName(this, "Victor", GetChannel());
 }
diff --git a/wpilibc/src/main/native/cpp/VictorSP.cpp b/wpilibc/src/main/native/cpp/VictorSP.cpp
index 82e966f..221777d 100644
--- a/wpilibc/src/main/native/cpp/VictorSP.cpp
+++ b/wpilibc/src/main/native/cpp/VictorSP.cpp
@@ -7,31 +7,18 @@
 
 #include "frc/VictorSP.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 VictorSP::VictorSP(int channel) : PWMSpeedController(channel) {
-  /* Note that the VictorSP uses the following bounds for PWM values. These
-   * values should work reasonably well for most controllers, but if users
-   * experience issues such as asymmetric behavior around the deadband or
-   * inability to saturate the controller in either direction, calibration is
-   * recommended. The calibration procedure can be found in the VictorSP User
-   * Manual available from Vex.
-   *
-   *   2.004ms = full "forward"
-   *   1.52ms = the "high end" of the deadband range
-   *   1.50ms = center of the deadband range (off)
-   *   1.48ms = the "low end" of the deadband range
-   *   0.997ms = full "reverse"
-   */
-  SetBounds(2.004, 1.52, 1.50, 1.48, .997);
+  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
   SetPeriodMultiplier(kPeriodMultiplier_1X);
   SetSpeed(0.0);
   SetZeroLatch();
 
-  HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel());
+  HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel() + 1);
   SendableRegistry::GetInstance().SetName(this, "VictorSP", GetChannel());
 }
diff --git a/wpilibc/src/main/native/cpp/XboxController.cpp b/wpilibc/src/main/native/cpp/XboxController.cpp
index fa54980..b294257 100644
--- a/wpilibc/src/main/native/cpp/XboxController.cpp
+++ b/wpilibc/src/main/native/cpp/XboxController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,12 +7,12 @@
 
 #include "frc/XboxController.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 using namespace frc;
 
 XboxController::XboxController(int port) : GenericHID(port) {
-  HAL_Report(HALUsageReporting::kResourceType_XboxController, port);
+  HAL_Report(HALUsageReporting::kResourceType_XboxController, port + 1);
 }
 
 double XboxController::GetX(JoystickHand hand) const {
diff --git a/wpilibc/src/main/native/cpp/buttons/Button.cpp b/wpilibc/src/main/native/cpp/buttons/Button.cpp
deleted file mode 100644
index 45ede65..0000000
--- a/wpilibc/src/main/native/cpp/buttons/Button.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/buttons/Button.h"
-
-using namespace frc;
-
-void Button::WhenPressed(Command* command) { WhenActive(command); }
-
-void Button::WhileHeld(Command* command) { WhileActive(command); }
-
-void Button::WhenReleased(Command* command) { WhenInactive(command); }
-
-void Button::CancelWhenPressed(Command* command) { CancelWhenActive(command); }
-
-void Button::ToggleWhenPressed(Command* command) { ToggleWhenActive(command); }
diff --git a/wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp
deleted file mode 100644
index 1e10255..0000000
--- a/wpilibc/src/main/native/cpp/buttons/ButtonScheduler.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/buttons/ButtonScheduler.h"
-
-#include "frc/commands/Scheduler.h"
-
-using namespace frc;
-
-ButtonScheduler::ButtonScheduler(bool last, Trigger* button, Command* orders)
-    : m_pressedLast(last), m_button(button), m_command(orders) {}
-
-void ButtonScheduler::Start() { Scheduler::GetInstance()->AddButton(this); }
diff --git a/wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
deleted file mode 100644
index 39d1d25..0000000
--- a/wpilibc/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/buttons/CancelButtonScheduler.h"
-
-#include "frc/buttons/Button.h"
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-CancelButtonScheduler::CancelButtonScheduler(bool last, Trigger* button,
-                                             Command* orders)
-    : ButtonScheduler(last, button, orders) {}
-
-void CancelButtonScheduler::Execute() {
-  bool pressed = m_button->Grab();
-
-  if (!m_pressedLast && pressed) {
-    m_command->Cancel();
-  }
-
-  m_pressedLast = pressed;
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
deleted file mode 100644
index feaa3c6..0000000
--- a/wpilibc/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/buttons/HeldButtonScheduler.h"
-
-#include "frc/buttons/Button.h"
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-HeldButtonScheduler::HeldButtonScheduler(bool last, Trigger* button,
-                                         Command* orders)
-    : ButtonScheduler(last, button, orders) {}
-
-void HeldButtonScheduler::Execute() {
-  bool pressed = m_button->Grab();
-
-  if (pressed) {
-    m_command->Start();
-  } else if (m_pressedLast && !pressed) {
-    m_command->Cancel();
-  }
-
-  m_pressedLast = pressed;
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/InternalButton.cpp b/wpilibc/src/main/native/cpp/buttons/InternalButton.cpp
deleted file mode 100644
index cac67c4..0000000
--- a/wpilibc/src/main/native/cpp/buttons/InternalButton.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/buttons/InternalButton.h"
-
-using namespace frc;
-
-InternalButton::InternalButton(bool inverted)
-    : m_pressed(inverted), m_inverted(inverted) {}
-
-void InternalButton::SetInverted(bool inverted) { m_inverted = inverted; }
-
-void InternalButton::SetPressed(bool pressed) { m_pressed = pressed; }
-
-bool InternalButton::Get() { return m_pressed ^ m_inverted; }
diff --git a/wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp b/wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp
deleted file mode 100644
index 2c93be2..0000000
--- a/wpilibc/src/main/native/cpp/buttons/JoystickButton.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/buttons/JoystickButton.h"
-
-using namespace frc;
-
-JoystickButton::JoystickButton(GenericHID* joystick, int buttonNumber)
-    : m_joystick(joystick), m_buttonNumber(buttonNumber) {}
-
-bool JoystickButton::Get() { return m_joystick->GetRawButton(m_buttonNumber); }
diff --git a/wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp b/wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp
deleted file mode 100644
index 5e8b1e0..0000000
--- a/wpilibc/src/main/native/cpp/buttons/NetworkButton.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/buttons/NetworkButton.h"
-
-#include <networktables/NetworkTable.h>
-#include <networktables/NetworkTableInstance.h>
-
-using namespace frc;
-
-NetworkButton::NetworkButton(const wpi::Twine& tableName,
-                             const wpi::Twine& field)
-    : NetworkButton(nt::NetworkTableInstance::GetDefault().GetTable(tableName),
-                    field) {}
-
-NetworkButton::NetworkButton(std::shared_ptr<nt::NetworkTable> table,
-                             const wpi::Twine& field)
-    : m_entry(table->GetEntry(field)) {}
-
-bool NetworkButton::Get() {
-  return m_entry.GetInstance().IsConnected() && m_entry.GetBoolean(false);
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/POVButton.cpp b/wpilibc/src/main/native/cpp/buttons/POVButton.cpp
deleted file mode 100644
index 6729923..0000000
--- a/wpilibc/src/main/native/cpp/buttons/POVButton.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 "frc/buttons/POVButton.h"
-
-using namespace frc;
-
-POVButton::POVButton(GenericHID& joystick, int angle, int povNumber)
-    : m_joystick(&joystick), m_angle(angle), m_povNumber(povNumber) {}
-
-bool POVButton::Get() { return m_joystick->GetPOV(m_povNumber) == m_angle; }
diff --git a/wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
deleted file mode 100644
index a964117..0000000
--- a/wpilibc/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/buttons/PressedButtonScheduler.h"
-
-#include "frc/buttons/Button.h"
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-PressedButtonScheduler::PressedButtonScheduler(bool last, Trigger* button,
-                                               Command* orders)
-    : ButtonScheduler(last, button, orders) {}
-
-void PressedButtonScheduler::Execute() {
-  bool pressed = m_button->Grab();
-
-  if (!m_pressedLast && pressed) {
-    m_command->Start();
-  }
-
-  m_pressedLast = pressed;
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
deleted file mode 100644
index 6d67004..0000000
--- a/wpilibc/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/buttons/ReleasedButtonScheduler.h"
-
-#include "frc/buttons/Button.h"
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-ReleasedButtonScheduler::ReleasedButtonScheduler(bool last, Trigger* button,
-                                                 Command* orders)
-    : ButtonScheduler(last, button, orders) {}
-
-void ReleasedButtonScheduler::Execute() {
-  bool pressed = m_button->Grab();
-
-  if (m_pressedLast && !pressed) {
-    m_command->Start();
-  }
-
-  m_pressedLast = pressed;
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp b/wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
deleted file mode 100644
index b722d45..0000000
--- a/wpilibc/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
+++ /dev/null
@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/buttons/ToggleButtonScheduler.h"
-
-#include "frc/buttons/Button.h"
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-ToggleButtonScheduler::ToggleButtonScheduler(bool last, Trigger* button,
-                                             Command* orders)
-    : ButtonScheduler(last, button, orders) {}
-
-void ToggleButtonScheduler::Execute() {
-  bool pressed = m_button->Grab();
-
-  if (!m_pressedLast && pressed) {
-    if (m_command->IsRunning()) {
-      m_command->Cancel();
-    } else {
-      m_command->Start();
-    }
-  }
-
-  m_pressedLast = pressed;
-}
diff --git a/wpilibc/src/main/native/cpp/buttons/Trigger.cpp b/wpilibc/src/main/native/cpp/buttons/Trigger.cpp
deleted file mode 100644
index f215083..0000000
--- a/wpilibc/src/main/native/cpp/buttons/Trigger.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/buttons/Button.h"
-#include "frc/buttons/CancelButtonScheduler.h"
-#include "frc/buttons/HeldButtonScheduler.h"
-#include "frc/buttons/PressedButtonScheduler.h"
-#include "frc/buttons/ReleasedButtonScheduler.h"
-#include "frc/buttons/ToggleButtonScheduler.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-
-using namespace frc;
-
-Trigger::Trigger(const Trigger& rhs) : SendableHelper(rhs) {}
-
-Trigger& Trigger::operator=(const Trigger& rhs) {
-  SendableHelper::operator=(rhs);
-  m_sendablePressed = false;
-  return *this;
-}
-
-Trigger::Trigger(Trigger&& rhs)
-    : SendableHelper(std::move(rhs)),
-      m_sendablePressed(rhs.m_sendablePressed.load()) {
-  rhs.m_sendablePressed = false;
-}
-
-Trigger& Trigger::operator=(Trigger&& rhs) {
-  SendableHelper::operator=(std::move(rhs));
-  m_sendablePressed = rhs.m_sendablePressed.load();
-  rhs.m_sendablePressed = false;
-  return *this;
-}
-
-bool Trigger::Grab() { return Get() || m_sendablePressed; }
-
-void Trigger::WhenActive(Command* command) {
-  auto pbs = new PressedButtonScheduler(Grab(), this, command);
-  pbs->Start();
-}
-
-void Trigger::WhileActive(Command* command) {
-  auto hbs = new HeldButtonScheduler(Grab(), this, command);
-  hbs->Start();
-}
-
-void Trigger::WhenInactive(Command* command) {
-  auto rbs = new ReleasedButtonScheduler(Grab(), this, command);
-  rbs->Start();
-}
-
-void Trigger::CancelWhenActive(Command* command) {
-  auto cbs = new CancelButtonScheduler(Grab(), this, command);
-  cbs->Start();
-}
-
-void Trigger::ToggleWhenActive(Command* command) {
-  auto tbs = new ToggleButtonScheduler(Grab(), this, command);
-  tbs->Start();
-}
-
-void Trigger::InitSendable(SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Button");
-  builder.SetSafeState([=]() { m_sendablePressed = false; });
-  builder.AddBooleanProperty("pressed", [=]() { return Grab(); },
-                             [=](bool value) { m_sendablePressed = value; });
-}
diff --git a/wpilibc/src/main/native/cpp/commands/Command.cpp b/wpilibc/src/main/native/cpp/commands/Command.cpp
deleted file mode 100644
index a7154c0..0000000
--- a/wpilibc/src/main/native/cpp/commands/Command.cpp
+++ /dev/null
@@ -1,264 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/commands/Command.h"
-
-#include <typeinfo>
-
-#include "frc/RobotState.h"
-#include "frc/Timer.h"
-#include "frc/WPIErrors.h"
-#include "frc/commands/CommandGroup.h"
-#include "frc/commands/Scheduler.h"
-#include "frc/livewindow/LiveWindow.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-int Command::m_commandCounter = 0;
-
-Command::Command() : Command("", -1.0) {}
-
-Command::Command(const wpi::Twine& name) : Command(name, -1.0) {}
-
-Command::Command(double timeout) : Command("", timeout) {}
-
-Command::Command(Subsystem& subsystem) : Command("", -1.0) {
-  Requires(&subsystem);
-}
-
-Command::Command(const wpi::Twine& name, double timeout) {
-  // We use -1.0 to indicate no timeout.
-  if (timeout < 0.0 && timeout != -1.0)
-    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
-
-  m_timeout = timeout;
-
-  // If name contains an empty string
-  if (name.isTriviallyEmpty() ||
-      (name.isSingleStringRef() && name.getSingleStringRef().empty())) {
-    SendableRegistry::GetInstance().Add(
-        this, "Command_" + wpi::Twine(typeid(*this).name()));
-  } else {
-    SendableRegistry::GetInstance().Add(this, name);
-  }
-}
-
-Command::Command(const wpi::Twine& name, Subsystem& subsystem)
-    : Command(name, -1.0) {
-  Requires(&subsystem);
-}
-
-Command::Command(double timeout, Subsystem& subsystem) : Command("", timeout) {
-  Requires(&subsystem);
-}
-
-Command::Command(const wpi::Twine& name, double timeout, Subsystem& subsystem)
-    : Command(name, timeout) {
-  Requires(&subsystem);
-}
-
-double Command::TimeSinceInitialized() const {
-  if (m_startTime < 0.0)
-    return 0.0;
-  else
-    return Timer::GetFPGATimestamp() - m_startTime;
-}
-
-void Command::Requires(Subsystem* subsystem) {
-  if (!AssertUnlocked("Can not add new requirement to command")) return;
-
-  if (subsystem != nullptr)
-    m_requirements.insert(subsystem);
-  else
-    wpi_setWPIErrorWithContext(NullParameter, "subsystem");
-}
-
-void Command::Start() {
-  LockChanges();
-  if (m_parent != nullptr)
-    wpi_setWPIErrorWithContext(
-        CommandIllegalUse,
-        "Can not start a command that is part of a command group");
-
-  m_completed = false;
-  Scheduler::GetInstance()->AddCommand(this);
-}
-
-bool Command::Run() {
-  if (!m_runWhenDisabled && m_parent == nullptr && RobotState::IsDisabled())
-    Cancel();
-
-  if (IsCanceled()) return false;
-
-  if (!m_initialized) {
-    m_initialized = true;
-    StartTiming();
-    _Initialize();
-    Initialize();
-  }
-  _Execute();
-  Execute();
-  return !IsFinished();
-}
-
-void Command::Cancel() {
-  if (m_parent != nullptr)
-    wpi_setWPIErrorWithContext(
-        CommandIllegalUse,
-        "Can not cancel a command that is part of a command group");
-
-  _Cancel();
-}
-
-bool Command::IsRunning() const { return m_running; }
-
-bool Command::IsInitialized() const { return m_initialized; }
-
-bool Command::IsCompleted() const { return m_completed; }
-
-bool Command::IsCanceled() const { return m_canceled; }
-
-bool Command::IsInterruptible() const { return m_interruptible; }
-
-void Command::SetInterruptible(bool interruptible) {
-  m_interruptible = interruptible;
-}
-
-bool Command::DoesRequire(Subsystem* system) const {
-  return m_requirements.count(system) > 0;
-}
-
-const Command::SubsystemSet& Command::GetRequirements() const {
-  return m_requirements;
-}
-
-CommandGroup* Command::GetGroup() const { return m_parent; }
-
-void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; }
-
-bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; }
-
-int Command::GetID() const { return m_commandID; }
-
-void Command::SetTimeout(double timeout) {
-  if (timeout < 0.0)
-    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
-  else
-    m_timeout = timeout;
-}
-
-bool Command::IsTimedOut() const {
-  return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
-}
-
-bool Command::AssertUnlocked(const std::string& message) {
-  if (m_locked) {
-    std::string buf =
-        message + " after being started or being added to a command group";
-    wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
-    return false;
-  }
-  return true;
-}
-
-void Command::SetParent(CommandGroup* parent) {
-  if (parent == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "parent");
-  } else if (m_parent != nullptr) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "Can not give command to a command group after "
-                               "already being put in a command group");
-  } else {
-    LockChanges();
-    m_parent = parent;
-  }
-}
-
-bool Command::IsParented() const { return m_parent != nullptr; }
-
-void Command::ClearRequirements() { m_requirements.clear(); }
-
-void Command::Initialize() {}
-
-void Command::Execute() {}
-
-void Command::End() {}
-
-void Command::Interrupted() { End(); }
-
-void Command::_Initialize() { m_completed = false; }
-
-void Command::_Interrupted() { m_completed = true; }
-
-void Command::_Execute() {}
-
-void Command::_End() { m_completed = true; }
-
-void Command::_Cancel() {
-  if (IsRunning()) m_canceled = true;
-}
-
-void Command::LockChanges() { m_locked = true; }
-
-void Command::Removed() {
-  if (m_initialized) {
-    if (IsCanceled()) {
-      Interrupted();
-      _Interrupted();
-    } else {
-      End();
-      _End();
-    }
-  }
-  m_initialized = false;
-  m_canceled = false;
-  m_running = false;
-  m_completed = true;
-}
-
-void Command::StartRunning() {
-  m_running = true;
-  m_startTime = -1;
-  m_completed = false;
-}
-
-void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); }
-
-std::string Command::GetName() const {
-  return SendableRegistry::GetInstance().GetName(this);
-}
-
-void Command::SetName(const wpi::Twine& name) {
-  SendableRegistry::GetInstance().SetName(this, name);
-}
-
-std::string Command::GetSubsystem() const {
-  return SendableRegistry::GetInstance().GetSubsystem(this);
-}
-
-void Command::SetSubsystem(const wpi::Twine& name) {
-  SendableRegistry::GetInstance().SetSubsystem(this, name);
-}
-
-void Command::InitSendable(SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Command");
-  builder.AddStringProperty(
-      ".name", [=]() { return SendableRegistry::GetInstance().GetName(this); },
-      nullptr);
-  builder.AddBooleanProperty("running", [=]() { return IsRunning(); },
-                             [=](bool value) {
-                               if (value) {
-                                 if (!IsRunning()) Start();
-                               } else {
-                                 if (IsRunning()) Cancel();
-                               }
-                             });
-  builder.AddBooleanProperty(".isParented", [=]() { return IsParented(); },
-                             nullptr);
-}
diff --git a/wpilibc/src/main/native/cpp/commands/CommandGroup.cpp b/wpilibc/src/main/native/cpp/commands/CommandGroup.cpp
deleted file mode 100644
index eac1746..0000000
--- a/wpilibc/src/main/native/cpp/commands/CommandGroup.cpp
+++ /dev/null
@@ -1,243 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/commands/CommandGroup.h"
-
-#include "frc/WPIErrors.h"
-
-using namespace frc;
-
-CommandGroup::CommandGroup(const wpi::Twine& name) : Command(name) {}
-
-void CommandGroup::AddSequential(Command* command) {
-  if (command == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "command");
-    return;
-  }
-  if (!AssertUnlocked("Cannot add new command to command group")) return;
-
-  m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence);
-
-  command->SetParent(this);
-
-  // Iterate through command->GetRequirements() and call Requires() on each
-  // required subsystem
-  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
-}
-
-void CommandGroup::AddSequential(Command* command, double timeout) {
-  if (command == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "command");
-    return;
-  }
-  if (!AssertUnlocked("Cannot add new command to command group")) return;
-  if (timeout < 0.0) {
-    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
-    return;
-  }
-
-  m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence,
-                          timeout);
-
-  command->SetParent(this);
-
-  // Iterate through command->GetRequirements() and call Requires() on each
-  // required subsystem
-  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
-}
-
-void CommandGroup::AddParallel(Command* command) {
-  if (command == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "command");
-    return;
-  }
-  if (!AssertUnlocked("Cannot add new command to command group")) return;
-
-  m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild);
-
-  command->SetParent(this);
-
-  // Iterate through command->GetRequirements() and call Requires() on each
-  // required subsystem
-  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
-}
-
-void CommandGroup::AddParallel(Command* command, double timeout) {
-  if (command == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "command");
-    return;
-  }
-  if (!AssertUnlocked("Cannot add new command to command group")) return;
-  if (timeout < 0.0) {
-    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
-    return;
-  }
-
-  m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild,
-                          timeout);
-
-  command->SetParent(this);
-
-  // Iterate through command->GetRequirements() and call Requires() on each
-  // required subsystem
-  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
-}
-
-bool CommandGroup::IsInterruptible() const {
-  if (!Command::IsInterruptible()) return false;
-
-  if (m_currentCommandIndex != -1 &&
-      static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
-    Command* cmd = m_commands[m_currentCommandIndex].m_command;
-    if (!cmd->IsInterruptible()) return false;
-  }
-
-  for (const auto& child : m_children) {
-    if (!child->m_command->IsInterruptible()) return false;
-  }
-
-  return true;
-}
-
-int CommandGroup::GetSize() const { return m_children.size(); }
-
-void CommandGroup::Initialize() {}
-
-void CommandGroup::Execute() {}
-
-bool CommandGroup::IsFinished() {
-  return static_cast<size_t>(m_currentCommandIndex) >= m_commands.size() &&
-         m_children.empty();
-}
-
-void CommandGroup::End() {}
-
-void CommandGroup::Interrupted() {}
-
-void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
-
-void CommandGroup::_Execute() {
-  CommandGroupEntry* entry;
-  Command* cmd = nullptr;
-  bool firstRun = false;
-
-  if (m_currentCommandIndex == -1) {
-    firstRun = true;
-    m_currentCommandIndex = 0;
-  }
-
-  // While there are still commands in this group to run
-  while (static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
-    // If a command is prepared to run
-    if (cmd != nullptr) {
-      // If command timed out, cancel it so it's removed from the Scheduler
-      if (entry->IsTimedOut()) cmd->_Cancel();
-
-      // If command finished or was cancelled, remove it from Scheduler
-      if (cmd->Run()) {
-        break;
-      } else {
-        cmd->Removed();
-
-        // Advance to next command in group
-        m_currentCommandIndex++;
-        firstRun = true;
-        cmd = nullptr;
-        continue;
-      }
-    }
-
-    entry = &m_commands[m_currentCommandIndex];
-    cmd = nullptr;
-
-    switch (entry->m_state) {
-      case CommandGroupEntry::kSequence_InSequence:
-        cmd = entry->m_command;
-        if (firstRun) {
-          cmd->StartRunning();
-          CancelConflicts(cmd);
-          firstRun = false;
-        }
-        break;
-
-      case CommandGroupEntry::kSequence_BranchPeer:
-        // Start executing a parallel command and advance to next entry in group
-        m_currentCommandIndex++;
-        entry->m_command->Start();
-        break;
-
-      case CommandGroupEntry::kSequence_BranchChild:
-        m_currentCommandIndex++;
-
-        /* Causes scheduler to skip children of current command which require
-         * the same subsystems as it
-         */
-        CancelConflicts(entry->m_command);
-        entry->m_command->StartRunning();
-
-        // Add current command entry to list of children of this group
-        m_children.push_back(entry);
-        break;
-    }
-  }
-
-  // Run Children
-  for (auto& entry : m_children) {
-    auto child = entry->m_command;
-    if (entry->IsTimedOut()) {
-      child->_Cancel();
-    }
-
-    // If child finished or was cancelled, set it to nullptr. nullptr entries
-    // are removed later.
-    if (!child->Run()) {
-      child->Removed();
-      entry = nullptr;
-    }
-  }
-
-  m_children.erase(std::remove(m_children.begin(), m_children.end(), nullptr),
-                   m_children.end());
-}
-
-void CommandGroup::_End() {
-  // Theoretically, we don't have to check this, but we do if teams override the
-  // IsFinished method
-  if (m_currentCommandIndex != -1 &&
-      static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
-    Command* cmd = m_commands[m_currentCommandIndex].m_command;
-    cmd->_Cancel();
-    cmd->Removed();
-  }
-
-  for (auto& child : m_children) {
-    Command* cmd = child->m_command;
-    cmd->_Cancel();
-    cmd->Removed();
-  }
-  m_children.clear();
-}
-
-void CommandGroup::_Interrupted() { _End(); }
-
-void CommandGroup::CancelConflicts(Command* command) {
-  for (auto childIter = m_children.begin(); childIter != m_children.end();) {
-    Command* child = (*childIter)->m_command;
-    bool erased = false;
-
-    for (auto&& requirement : command->GetRequirements()) {
-      if (child->DoesRequire(requirement)) {
-        child->_Cancel();
-        child->Removed();
-        childIter = m_children.erase(childIter);
-        erased = true;
-        break;
-      }
-    }
-    if (!erased) childIter++;
-  }
-}
diff --git a/wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp b/wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp
deleted file mode 100644
index 7a75f00..0000000
--- a/wpilibc/src/main/native/cpp/commands/CommandGroupEntry.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/commands/CommandGroupEntry.h"
-
-#include "frc/commands/Command.h"
-
-using namespace frc;
-
-CommandGroupEntry::CommandGroupEntry(Command* command, Sequence state,
-                                     double timeout)
-    : m_timeout(timeout), m_command(command), m_state(state) {}
-
-bool CommandGroupEntry::IsTimedOut() const {
-  if (m_timeout < 0.0) return false;
-  double time = m_command->TimeSinceInitialized();
-  if (time == 0.0) return false;
-  return time >= m_timeout;
-}
diff --git a/wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp b/wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp
deleted file mode 100644
index c44f7ba..0000000
--- a/wpilibc/src/main/native/cpp/commands/ConditionalCommand.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/commands/ConditionalCommand.h"
-
-#include "frc/commands/Scheduler.h"
-
-using namespace frc;
-
-static void RequireAll(Command& command, Command* onTrue, Command* onFalse) {
-  if (onTrue != nullptr) {
-    for (auto requirement : onTrue->GetRequirements())
-      command.Requires(requirement);
-  }
-  if (onFalse != nullptr) {
-    for (auto requirement : onFalse->GetRequirements())
-      command.Requires(requirement);
-  }
-}
-
-ConditionalCommand::ConditionalCommand(Command* onTrue, Command* onFalse) {
-  m_onTrue = onTrue;
-  m_onFalse = onFalse;
-
-  RequireAll(*this, onTrue, onFalse);
-}
-
-ConditionalCommand::ConditionalCommand(const wpi::Twine& name, Command* onTrue,
-                                       Command* onFalse)
-    : Command(name) {
-  m_onTrue = onTrue;
-  m_onFalse = onFalse;
-
-  RequireAll(*this, onTrue, onFalse);
-}
-
-void ConditionalCommand::_Initialize() {
-  if (Condition()) {
-    m_chosenCommand = m_onTrue;
-  } else {
-    m_chosenCommand = m_onFalse;
-  }
-
-  if (m_chosenCommand != nullptr) {
-    // This is a hack to make cancelling the chosen command inside a
-    // CommandGroup work properly
-    m_chosenCommand->ClearRequirements();
-
-    m_chosenCommand->Start();
-  }
-  Command::_Initialize();
-}
-
-void ConditionalCommand::_Cancel() {
-  if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
-    m_chosenCommand->Cancel();
-  }
-
-  Command::_Cancel();
-}
-
-bool ConditionalCommand::IsFinished() {
-  if (m_chosenCommand != nullptr) {
-    return m_chosenCommand->IsCompleted();
-  } else {
-    return true;
-  }
-}
-
-void ConditionalCommand::_Interrupted() {
-  if (m_chosenCommand != nullptr && m_chosenCommand->IsRunning()) {
-    m_chosenCommand->Cancel();
-  }
-
-  Command::_Interrupted();
-}
diff --git a/wpilibc/src/main/native/cpp/commands/InstantCommand.cpp b/wpilibc/src/main/native/cpp/commands/InstantCommand.cpp
deleted file mode 100644
index 88ebfde..0000000
--- a/wpilibc/src/main/native/cpp/commands/InstantCommand.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-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 "frc/commands/InstantCommand.h"
-
-using namespace frc;
-
-InstantCommand::InstantCommand(const wpi::Twine& name) : Command(name) {}
-
-InstantCommand::InstantCommand(Subsystem& subsystem) : Command(subsystem) {}
-
-InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem)
-    : Command(name, subsystem) {}
-
-InstantCommand::InstantCommand(std::function<void()> func) : m_func(func) {}
-
-InstantCommand::InstantCommand(Subsystem& subsystem, std::function<void()> func)
-    : InstantCommand(subsystem) {
-  m_func = func;
-}
-
-InstantCommand::InstantCommand(const wpi::Twine& name,
-                               std::function<void()> func)
-    : InstantCommand(name) {
-  m_func = func;
-}
-
-InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
-                               std::function<void()> func)
-    : InstantCommand(name, subsystem) {
-  m_func = func;
-}
-
-void InstantCommand::_Initialize() {
-  Command::_Initialize();
-  if (m_func) {
-    m_func();
-  }
-}
-
-bool InstantCommand::IsFinished() { return true; }
diff --git a/wpilibc/src/main/native/cpp/commands/PIDCommand.cpp b/wpilibc/src/main/native/cpp/commands/PIDCommand.cpp
deleted file mode 100644
index 875d8fe..0000000
--- a/wpilibc/src/main/native/cpp/commands/PIDCommand.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/commands/PIDCommand.h"
-
-#include "frc/smartdashboard/SendableBuilder.h"
-
-using namespace frc;
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
-                       double f, double period)
-    : Command(name) {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, double f, double period) {
-  m_controller =
-      std::make_shared<PIDController>(p, i, d, f, this, this, period);
-}
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d)
-    : Command(name) {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
-}
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
-                       double period)
-    : Command(name) {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d) {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, double period) {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
-                       double f, double period, Subsystem& subsystem)
-    : Command(name, subsystem) {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, double f, double period,
-                       Subsystem& subsystem)
-    : Command(subsystem) {
-  m_controller =
-      std::make_shared<PIDController>(p, i, d, f, this, this, period);
-}
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
-                       Subsystem& subsystem)
-    : Command(name, subsystem) {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
-}
-
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
-                       double period, Subsystem& subsystem)
-    : Command(name, subsystem) {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, Subsystem& subsystem) {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
-}
-
-PIDCommand::PIDCommand(double p, double i, double d, double period,
-                       Subsystem& subsystem) {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
-}
-
-void PIDCommand::_Initialize() { m_controller->Enable(); }
-
-void PIDCommand::_End() { m_controller->Disable(); }
-
-void PIDCommand::_Interrupted() { _End(); }
-
-void PIDCommand::SetSetpointRelative(double deltaSetpoint) {
-  SetSetpoint(GetSetpoint() + deltaSetpoint);
-}
-
-void PIDCommand::PIDWrite(double output) { UsePIDOutput(output); }
-
-double PIDCommand::PIDGet() { return ReturnPIDInput(); }
-
-std::shared_ptr<PIDController> PIDCommand::GetPIDController() const {
-  return m_controller;
-}
-
-void PIDCommand::SetSetpoint(double setpoint) {
-  m_controller->SetSetpoint(setpoint);
-}
-
-double PIDCommand::GetSetpoint() const { return m_controller->GetSetpoint(); }
-
-double PIDCommand::GetPosition() { return ReturnPIDInput(); }
-
-void PIDCommand::InitSendable(SendableBuilder& builder) {
-  m_controller->InitSendable(builder);
-  Command::InitSendable(builder);
-  builder.SetSmartDashboardType("PIDCommand");
-}
diff --git a/wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp b/wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp
deleted file mode 100644
index 0893da3..0000000
--- a/wpilibc/src/main/native/cpp/commands/PIDSubsystem.cpp
+++ /dev/null
@@ -1,97 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/commands/PIDSubsystem.h"
-
-#include "frc/PIDController.h"
-
-using namespace frc;
-
-PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d)
-    : Subsystem(name) {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
-  AddChild("PIDController", m_controller);
-}
-
-PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
-                           double f)
-    : Subsystem(name) {
-  m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
-  AddChild("PIDController", m_controller);
-}
-
-PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
-                           double f, double period)
-    : Subsystem(name) {
-  m_controller =
-      std::make_shared<PIDController>(p, i, d, f, this, this, period);
-  AddChild("PIDController", m_controller);
-}
-
-PIDSubsystem::PIDSubsystem(double p, double i, double d)
-    : Subsystem("PIDSubsystem") {
-  m_controller = std::make_shared<PIDController>(p, i, d, this, this);
-  AddChild("PIDController", m_controller);
-}
-
-PIDSubsystem::PIDSubsystem(double p, double i, double d, double f)
-    : Subsystem("PIDSubsystem") {
-  m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
-  AddChild("PIDController", m_controller);
-}
-
-PIDSubsystem::PIDSubsystem(double p, double i, double d, double f,
-                           double period)
-    : Subsystem("PIDSubsystem") {
-  m_controller =
-      std::make_shared<PIDController>(p, i, d, f, this, this, period);
-  AddChild("PIDController", m_controller);
-}
-
-void PIDSubsystem::Enable() { m_controller->Enable(); }
-
-void PIDSubsystem::Disable() { m_controller->Disable(); }
-
-void PIDSubsystem::PIDWrite(double output) { UsePIDOutput(output); }
-
-double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
-
-void PIDSubsystem::SetSetpoint(double setpoint) {
-  m_controller->SetSetpoint(setpoint);
-}
-
-void PIDSubsystem::SetSetpointRelative(double deltaSetpoint) {
-  SetSetpoint(GetSetpoint() + deltaSetpoint);
-}
-
-void PIDSubsystem::SetInputRange(double minimumInput, double maximumInput) {
-  m_controller->SetInputRange(minimumInput, maximumInput);
-}
-
-void PIDSubsystem::SetOutputRange(double minimumOutput, double maximumOutput) {
-  m_controller->SetOutputRange(minimumOutput, maximumOutput);
-}
-
-double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); }
-
-double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
-
-double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
-
-void PIDSubsystem::SetAbsoluteTolerance(double absValue) {
-  m_controller->SetAbsoluteTolerance(absValue);
-}
-
-void PIDSubsystem::SetPercentTolerance(double percent) {
-  m_controller->SetPercentTolerance(percent);
-}
-
-bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
-
-std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
-  return m_controller;
-}
diff --git a/wpilibc/src/main/native/cpp/commands/PrintCommand.cpp b/wpilibc/src/main/native/cpp/commands/PrintCommand.cpp
deleted file mode 100644
index b4bea24..0000000
--- a/wpilibc/src/main/native/cpp/commands/PrintCommand.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/commands/PrintCommand.h"
-
-#include <wpi/raw_ostream.h>
-
-using namespace frc;
-
-PrintCommand::PrintCommand(const wpi::Twine& message)
-    : InstantCommand("Print \"" + message + wpi::Twine('"')) {
-  m_message = message.str();
-}
-
-void PrintCommand::Initialize() { wpi::outs() << m_message << '\n'; }
diff --git a/wpilibc/src/main/native/cpp/commands/Scheduler.cpp b/wpilibc/src/main/native/cpp/commands/Scheduler.cpp
deleted file mode 100644
index f406d74..0000000
--- a/wpilibc/src/main/native/cpp/commands/Scheduler.cpp
+++ /dev/null
@@ -1,245 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/commands/Scheduler.h"
-
-#include <algorithm>
-#include <set>
-#include <string>
-#include <vector>
-
-#include <hal/HAL.h>
-#include <networktables/NetworkTableEntry.h>
-#include <wpi/mutex.h>
-
-#include "frc/WPIErrors.h"
-#include "frc/buttons/ButtonScheduler.h"
-#include "frc/commands/Command.h"
-#include "frc/commands/Subsystem.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-struct Scheduler::Impl {
-  void Remove(Command* command);
-  void ProcessCommandAddition(Command* command);
-
-  using SubsystemSet = std::set<Subsystem*>;
-  SubsystemSet subsystems;
-  wpi::mutex buttonsMutex;
-  using ButtonVector = std::vector<std::unique_ptr<ButtonScheduler>>;
-  ButtonVector buttons;
-  using CommandVector = std::vector<Command*>;
-  wpi::mutex additionsMutex;
-  CommandVector additions;
-  using CommandSet = std::set<Command*>;
-  CommandSet commands;
-  bool adding = false;
-  bool enabled = true;
-  std::vector<std::string> commandsBuf;
-  std::vector<double> idsBuf;
-  bool runningCommandsChanged = false;
-};
-
-Scheduler* Scheduler::GetInstance() {
-  static Scheduler instance;
-  return &instance;
-}
-
-void Scheduler::AddCommand(Command* command) {
-  std::scoped_lock lock(m_impl->additionsMutex);
-  if (std::find(m_impl->additions.begin(), m_impl->additions.end(), command) !=
-      m_impl->additions.end())
-    return;
-  m_impl->additions.push_back(command);
-}
-
-void Scheduler::AddButton(ButtonScheduler* button) {
-  std::scoped_lock lock(m_impl->buttonsMutex);
-  m_impl->buttons.emplace_back(button);
-}
-
-void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
-  if (subsystem == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "subsystem");
-    return;
-  }
-  m_impl->subsystems.insert(subsystem);
-}
-
-void Scheduler::Run() {
-  // Get button input (going backwards preserves button priority)
-  {
-    if (!m_impl->enabled) return;
-
-    std::scoped_lock lock(m_impl->buttonsMutex);
-    for (auto& button : m_impl->buttons) {
-      button->Execute();
-    }
-  }
-
-  // Call every subsystem's periodic method
-  for (auto& subsystem : m_impl->subsystems) {
-    subsystem->Periodic();
-  }
-
-  m_impl->runningCommandsChanged = false;
-
-  // Loop through the commands
-  for (auto cmdIter = m_impl->commands.begin();
-       cmdIter != m_impl->commands.end();) {
-    Command* command = *cmdIter;
-    // Increment before potentially removing to keep the iterator valid
-    ++cmdIter;
-    if (!command->Run()) {
-      Remove(command);
-      m_impl->runningCommandsChanged = true;
-    }
-  }
-
-  // Add the new things
-  {
-    std::scoped_lock lock(m_impl->additionsMutex);
-    for (auto& addition : m_impl->additions) {
-      // Check to make sure no adding during adding
-      if (m_impl->adding) {
-        wpi_setWPIErrorWithContext(IncompatibleState,
-                                   "Can not start command from cancel method");
-      } else {
-        m_impl->ProcessCommandAddition(addition);
-      }
-    }
-    m_impl->additions.clear();
-  }
-
-  // Add in the defaults
-  for (auto& subsystem : m_impl->subsystems) {
-    if (subsystem->GetCurrentCommand() == nullptr) {
-      if (m_impl->adding) {
-        wpi_setWPIErrorWithContext(IncompatibleState,
-                                   "Can not start command from cancel method");
-      } else {
-        m_impl->ProcessCommandAddition(subsystem->GetDefaultCommand());
-      }
-    }
-    subsystem->ConfirmCommand();
-  }
-}
-
-void Scheduler::Remove(Command* command) {
-  if (command == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "command");
-    return;
-  }
-
-  m_impl->Remove(command);
-}
-
-void Scheduler::RemoveAll() {
-  while (m_impl->commands.size() > 0) {
-    Remove(*m_impl->commands.begin());
-  }
-}
-
-void Scheduler::ResetAll() {
-  RemoveAll();
-  m_impl->subsystems.clear();
-  m_impl->buttons.clear();
-  m_impl->additions.clear();
-  m_impl->commands.clear();
-}
-
-void Scheduler::SetEnabled(bool enabled) { m_impl->enabled = enabled; }
-
-void Scheduler::InitSendable(SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Scheduler");
-  auto namesEntry = builder.GetEntry("Names");
-  auto idsEntry = builder.GetEntry("Ids");
-  auto cancelEntry = builder.GetEntry("Cancel");
-  builder.SetUpdateTable([=]() {
-    // Get the list of possible commands to cancel
-    auto new_toCancel = cancelEntry.GetValue();
-    wpi::ArrayRef<double> toCancel;
-    if (new_toCancel) toCancel = new_toCancel->GetDoubleArray();
-
-    // Cancel commands whose cancel buttons were pressed on the SmartDashboard
-    if (!toCancel.empty()) {
-      for (auto& command : m_impl->commands) {
-        for (const auto& cancelled : toCancel) {
-          if (command->GetID() == cancelled) {
-            command->Cancel();
-          }
-        }
-      }
-      nt::NetworkTableEntry(cancelEntry).SetDoubleArray({});
-    }
-
-    // Set the running commands
-    if (m_impl->runningCommandsChanged) {
-      m_impl->commandsBuf.resize(0);
-      m_impl->idsBuf.resize(0);
-      auto& registry = SendableRegistry::GetInstance();
-      for (const auto& command : m_impl->commands) {
-        m_impl->commandsBuf.emplace_back(registry.GetName(command));
-        m_impl->idsBuf.emplace_back(command->GetID());
-      }
-      nt::NetworkTableEntry(namesEntry).SetStringArray(m_impl->commandsBuf);
-      nt::NetworkTableEntry(idsEntry).SetDoubleArray(m_impl->idsBuf);
-    }
-  });
-}
-
-Scheduler::Scheduler() : m_impl(new Impl) {
-  HAL_Report(HALUsageReporting::kResourceType_Command,
-             HALUsageReporting::kCommand_Scheduler);
-  SendableRegistry::GetInstance().AddLW(this, "Scheduler");
-}
-
-Scheduler::~Scheduler() {}
-
-void Scheduler::Impl::Remove(Command* command) {
-  if (!commands.erase(command)) return;
-
-  for (auto&& requirement : command->GetRequirements()) {
-    requirement->SetCurrentCommand(nullptr);
-  }
-
-  command->Removed();
-}
-
-void Scheduler::Impl::ProcessCommandAddition(Command* command) {
-  if (command == nullptr) return;
-
-  // Only add if not already in
-  auto found = commands.find(command);
-  if (found == commands.end()) {
-    // Check that the requirements can be had
-    const auto& requirements = command->GetRequirements();
-    for (const auto& requirement : requirements) {
-      if (requirement->GetCurrentCommand() != nullptr &&
-          !requirement->GetCurrentCommand()->IsInterruptible())
-        return;
-    }
-
-    // Give it the requirements
-    adding = true;
-    for (auto&& requirement : requirements) {
-      if (requirement->GetCurrentCommand() != nullptr) {
-        requirement->GetCurrentCommand()->Cancel();
-        Remove(requirement->GetCurrentCommand());
-      }
-      requirement->SetCurrentCommand(command);
-    }
-    adding = false;
-
-    commands.insert(command);
-
-    command->StartRunning();
-    runningCommandsChanged = true;
-  }
-}
diff --git a/wpilibc/src/main/native/cpp/commands/StartCommand.cpp b/wpilibc/src/main/native/cpp/commands/StartCommand.cpp
deleted file mode 100644
index 8884124..0000000
--- a/wpilibc/src/main/native/cpp/commands/StartCommand.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/commands/StartCommand.h"
-
-using namespace frc;
-
-StartCommand::StartCommand(Command* commandToStart)
-    : InstantCommand("StartCommand") {
-  m_commandToFork = commandToStart;
-}
-
-void StartCommand::Initialize() { m_commandToFork->Start(); }
diff --git a/wpilibc/src/main/native/cpp/commands/Subsystem.cpp b/wpilibc/src/main/native/cpp/commands/Subsystem.cpp
deleted file mode 100644
index 6e665ea..0000000
--- a/wpilibc/src/main/native/cpp/commands/Subsystem.cpp
+++ /dev/null
@@ -1,134 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/commands/Subsystem.h"
-
-#include "frc/WPIErrors.h"
-#include "frc/commands/Command.h"
-#include "frc/commands/Scheduler.h"
-#include "frc/livewindow/LiveWindow.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-Subsystem::Subsystem(const wpi::Twine& name) {
-  SendableRegistry::GetInstance().AddLW(this, name, name);
-  Scheduler::GetInstance()->RegisterSubsystem(this);
-}
-
-void Subsystem::SetDefaultCommand(Command* command) {
-  if (command == nullptr) {
-    m_defaultCommand = nullptr;
-  } else {
-    const auto& reqs = command->GetRequirements();
-    if (std::find(reqs.begin(), reqs.end(), this) == reqs.end()) {
-      wpi_setWPIErrorWithContext(
-          CommandIllegalUse, "A default command must require the subsystem");
-      return;
-    }
-
-    m_defaultCommand = command;
-  }
-}
-
-Command* Subsystem::GetDefaultCommand() {
-  if (!m_initializedDefaultCommand) {
-    m_initializedDefaultCommand = true;
-    InitDefaultCommand();
-  }
-  return m_defaultCommand;
-}
-
-wpi::StringRef Subsystem::GetDefaultCommandName() {
-  Command* defaultCommand = GetDefaultCommand();
-  if (defaultCommand) {
-    return SendableRegistry::GetInstance().GetName(defaultCommand);
-  } else {
-    return wpi::StringRef();
-  }
-}
-
-void Subsystem::SetCurrentCommand(Command* command) {
-  m_currentCommand = command;
-  m_currentCommandChanged = true;
-}
-
-Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; }
-
-wpi::StringRef Subsystem::GetCurrentCommandName() const {
-  Command* currentCommand = GetCurrentCommand();
-  if (currentCommand) {
-    return SendableRegistry::GetInstance().GetName(currentCommand);
-  } else {
-    return wpi::StringRef();
-  }
-}
-
-void Subsystem::Periodic() {}
-
-void Subsystem::InitDefaultCommand() {}
-
-std::string Subsystem::GetName() const {
-  return SendableRegistry::GetInstance().GetName(this);
-}
-
-void Subsystem::SetName(const wpi::Twine& name) {
-  SendableRegistry::GetInstance().SetName(this, name);
-}
-
-std::string Subsystem::GetSubsystem() const {
-  return SendableRegistry::GetInstance().GetSubsystem(this);
-}
-
-void Subsystem::SetSubsystem(const wpi::Twine& name) {
-  SendableRegistry::GetInstance().SetSubsystem(this, name);
-}
-
-void Subsystem::AddChild(const wpi::Twine& name,
-                         std::shared_ptr<Sendable> child) {
-  AddChild(name, *child);
-}
-
-void Subsystem::AddChild(const wpi::Twine& name, Sendable* child) {
-  AddChild(name, *child);
-}
-
-void Subsystem::AddChild(const wpi::Twine& name, Sendable& child) {
-  auto& registry = SendableRegistry::GetInstance();
-  registry.AddLW(&child, registry.GetSubsystem(this), name);
-  registry.AddChild(this, &child);
-}
-
-void Subsystem::AddChild(std::shared_ptr<Sendable> child) { AddChild(*child); }
-
-void Subsystem::AddChild(Sendable* child) { AddChild(*child); }
-
-void Subsystem::AddChild(Sendable& child) {
-  auto& registry = SendableRegistry::GetInstance();
-  registry.SetSubsystem(&child, registry.GetSubsystem(this));
-  registry.EnableLiveWindow(&child);
-  registry.AddChild(this, &child);
-}
-
-void Subsystem::ConfirmCommand() {
-  if (m_currentCommandChanged) m_currentCommandChanged = false;
-}
-
-void Subsystem::InitSendable(SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Subsystem");
-
-  builder.AddBooleanProperty(
-      ".hasDefault", [=]() { return m_defaultCommand != nullptr; }, nullptr);
-  builder.AddStringProperty(".default",
-                            [=]() { return GetDefaultCommandName(); }, nullptr);
-
-  builder.AddBooleanProperty(
-      ".hasCommand", [=]() { return m_currentCommand != nullptr; }, nullptr);
-  builder.AddStringProperty(".command",
-                            [=]() { return GetCurrentCommandName(); }, nullptr);
-}
diff --git a/wpilibc/src/main/native/cpp/commands/TimedCommand.cpp b/wpilibc/src/main/native/cpp/commands/TimedCommand.cpp
deleted file mode 100644
index 122751a..0000000
--- a/wpilibc/src/main/native/cpp/commands/TimedCommand.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-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 "frc/commands/TimedCommand.h"
-
-using namespace frc;
-
-TimedCommand::TimedCommand(const wpi::Twine& name, double timeout)
-    : Command(name, timeout) {}
-
-TimedCommand::TimedCommand(double timeout) : Command(timeout) {}
-
-TimedCommand::TimedCommand(const wpi::Twine& name, double timeout,
-                           Subsystem& subsystem)
-    : Command(name, timeout, subsystem) {}
-
-TimedCommand::TimedCommand(double timeout, Subsystem& subsystem)
-    : Command(timeout, subsystem) {}
-
-bool TimedCommand::IsFinished() { return IsTimedOut(); }
diff --git a/wpilibc/src/main/native/cpp/commands/WaitCommand.cpp b/wpilibc/src/main/native/cpp/commands/WaitCommand.cpp
deleted file mode 100644
index 225d95b..0000000
--- a/wpilibc/src/main/native/cpp/commands/WaitCommand.cpp
+++ /dev/null
@@ -1,16 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/commands/WaitCommand.h"
-
-using namespace frc;
-
-WaitCommand::WaitCommand(double timeout)
-    : TimedCommand("Wait(" + std::to_string(timeout) + ")", timeout) {}
-
-WaitCommand::WaitCommand(const wpi::Twine& name, double timeout)
-    : TimedCommand(name, timeout) {}
diff --git a/wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp b/wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp
deleted file mode 100644
index 5c99c1b..0000000
--- a/wpilibc/src/main/native/cpp/commands/WaitForChildren.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/commands/WaitForChildren.h"
-
-#include "frc/commands/CommandGroup.h"
-
-using namespace frc;
-
-WaitForChildren::WaitForChildren(double timeout)
-    : Command("WaitForChildren", timeout) {}
-
-WaitForChildren::WaitForChildren(const wpi::Twine& name, double timeout)
-    : Command(name, timeout) {}
-
-bool WaitForChildren::IsFinished() {
-  return GetGroup() == nullptr || GetGroup()->GetSize() == 0;
-}
diff --git a/wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp b/wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp
deleted file mode 100644
index 6e24b6b..0000000
--- a/wpilibc/src/main/native/cpp/commands/WaitUntilCommand.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-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 "frc/commands/WaitUntilCommand.h"
-
-#include "frc/Timer.h"
-
-using namespace frc;
-
-WaitUntilCommand::WaitUntilCommand(double time)
-    : Command("WaitUntilCommand", time) {
-  m_time = time;
-}
-
-WaitUntilCommand::WaitUntilCommand(const wpi::Twine& name, double time)
-    : Command(name, time) {
-  m_time = time;
-}
-
-bool WaitUntilCommand::IsFinished() { return Timer::GetMatchTime() >= m_time; }
diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
index 2202d93..264859f 100644
--- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
@@ -10,7 +10,7 @@
 #include <algorithm>
 #include <cmath>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
diff --git a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp b/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
deleted file mode 100644
index 4ef3bf9..0000000
--- a/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/controller/ProfiledPIDController.h"
-
-#include <algorithm>
-#include <cmath>
-
-#include "frc/smartdashboard/SendableBuilder.h"
-
-using namespace frc;
-
-ProfiledPIDController::ProfiledPIDController(
-    double Kp, double Ki, double Kd,
-    frc::TrapezoidProfile::Constraints constraints, units::second_t period)
-    : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
-
-void ProfiledPIDController::SetP(double Kp) { m_controller.SetP(Kp); }
-
-void ProfiledPIDController::SetI(double Ki) { m_controller.SetI(Ki); }
-
-void ProfiledPIDController::SetD(double Kd) { m_controller.SetD(Kd); }
-
-double ProfiledPIDController::GetP() const { return m_controller.GetP(); }
-
-double ProfiledPIDController::GetI() const { return m_controller.GetI(); }
-
-double ProfiledPIDController::GetD() const { return m_controller.GetD(); }
-
-units::second_t ProfiledPIDController::GetPeriod() const {
-  return m_controller.GetPeriod();
-}
-
-void ProfiledPIDController::SetGoal(TrapezoidProfile::State goal) {
-  m_goal = goal;
-}
-
-void ProfiledPIDController::SetGoal(units::meter_t goal) {
-  m_goal = {goal, 0_mps};
-}
-
-TrapezoidProfile::State ProfiledPIDController::GetGoal() const {
-  return m_goal;
-}
-
-bool ProfiledPIDController::AtGoal() const {
-  return AtSetpoint() && m_goal == m_setpoint;
-}
-
-void ProfiledPIDController::SetConstraints(
-    frc::TrapezoidProfile::Constraints constraints) {
-  m_constraints = constraints;
-}
-
-TrapezoidProfile::State ProfiledPIDController::GetSetpoint() const {
-  return m_setpoint;
-}
-
-bool ProfiledPIDController::AtSetpoint() const {
-  return m_controller.AtSetpoint();
-}
-
-void ProfiledPIDController::EnableContinuousInput(double minimumInput,
-                                                  double maximumInput) {
-  m_controller.EnableContinuousInput(minimumInput, maximumInput);
-}
-
-void ProfiledPIDController::DisableContinuousInput() {
-  m_controller.DisableContinuousInput();
-}
-
-void ProfiledPIDController::SetIntegratorRange(double minimumIntegral,
-                                               double maximumIntegral) {
-  m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
-}
-
-void ProfiledPIDController::SetTolerance(double positionTolerance,
-                                         double velocityTolerance) {
-  m_controller.SetTolerance(positionTolerance, velocityTolerance);
-}
-
-double ProfiledPIDController::GetPositionError() const {
-  return m_controller.GetPositionError();
-}
-
-double ProfiledPIDController::GetVelocityError() const {
-  return m_controller.GetVelocityError();
-}
-
-double ProfiledPIDController::Calculate(units::meter_t measurement) {
-  frc::TrapezoidProfile profile{m_constraints, m_goal, m_setpoint};
-  m_setpoint = profile.Calculate(GetPeriod());
-  return m_controller.Calculate(measurement.to<double>(),
-                                m_setpoint.position.to<double>());
-}
-
-double ProfiledPIDController::Calculate(units::meter_t measurement,
-                                        TrapezoidProfile::State goal) {
-  SetGoal(goal);
-  return Calculate(measurement);
-}
-
-double ProfiledPIDController::Calculate(units::meter_t measurement,
-                                        units::meter_t goal) {
-  SetGoal(goal);
-  return Calculate(measurement);
-}
-
-double ProfiledPIDController::Calculate(
-    units::meter_t measurement, units::meter_t goal,
-    frc::TrapezoidProfile::Constraints constraints) {
-  SetConstraints(constraints);
-  return Calculate(measurement, goal);
-}
-
-void ProfiledPIDController::Reset() { m_controller.Reset(); }
-
-void ProfiledPIDController::InitSendable(frc::SendableBuilder& builder) {
-  builder.SetSmartDashboardType("ProfiledPIDController");
-  builder.AddDoubleProperty("p", [this] { return GetP(); },
-                            [this](double value) { SetP(value); });
-  builder.AddDoubleProperty("i", [this] { return GetI(); },
-                            [this](double value) { SetI(value); });
-  builder.AddDoubleProperty("d", [this] { return GetD(); },
-                            [this](double value) { SetD(value); });
-  builder.AddDoubleProperty(
-      "goal", [this] { return GetGoal().position.to<double>(); },
-      [this](double value) { SetGoal(units::meter_t{value}); });
-}
diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index ee23a5e..2d30a2b 100644
--- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -10,7 +10,7 @@
 #include <algorithm>
 #include <cmath>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/SpeedController.h"
 #include "frc/smartdashboard/SendableBuilder.h"
diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
index 4c15de8..e03951b 100644
--- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -10,7 +10,7 @@
 #include <algorithm>
 #include <cmath>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <wpi/math>
 
 #include "frc/SpeedController.h"
diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index 473e033..0102d6a 100644
--- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -10,7 +10,7 @@
 #include <algorithm>
 #include <cmath>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <wpi/math>
 
 #include "frc/SpeedController.h"
diff --git a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
index bae8868..fbce5a1 100644
--- a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
+++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
@@ -11,7 +11,7 @@
 #include <cmath>
 #include <cstddef>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/Base.h"
 #include "frc/SpeedController.h"
diff --git a/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp b/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
index 7dfc8f0..434cc27 100644
--- a/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
+++ b/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
@@ -10,7 +10,7 @@
 #include <cassert>
 #include <cmath>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 using namespace frc;
 
diff --git a/wpilibc/src/main/native/cpp/frc2/Timer.cpp b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
index 126cfdd..76721bc 100644
--- a/wpilibc/src/main/native/cpp/frc2/Timer.cpp
+++ b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
@@ -10,7 +10,7 @@
 #include <chrono>
 #include <thread>
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 
 #include "frc/DriverStation.h"
 #include "frc/RobotController.h"
@@ -129,7 +129,7 @@
 
 units::second_t Timer::GetFPGATimestamp() {
   // FPGA returns the timestamp in microseconds
-  return units::second_t(frc::RobotController::GetFPGATime()) * 1.0e-6;
+  return units::second_t(frc::RobotController::GetFPGATime() * 1.0e-6);
 }
 
 units::second_t Timer::GetMatchTime() {
diff --git a/wpilibc/src/main/native/cpp/frc2/command/Command.cpp b/wpilibc/src/main/native/cpp/frc2/command/Command.cpp
deleted file mode 100644
index 692879c..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/Command.cpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/Command.h"
-
-#include <iostream>
-
-#include "frc2/command/CommandScheduler.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/ParallelCommandGroup.h"
-#include "frc2/command/ParallelDeadlineGroup.h"
-#include "frc2/command/ParallelRaceGroup.h"
-#include "frc2/command/PerpetualCommand.h"
-#include "frc2/command/ProxyScheduleCommand.h"
-#include "frc2/command/SequentialCommandGroup.h"
-#include "frc2/command/WaitCommand.h"
-#include "frc2/command/WaitUntilCommand.h"
-
-using namespace frc2;
-
-Command::~Command() { CommandScheduler::GetInstance().Cancel(this); }
-
-Command::Command(const Command& rhs) : ErrorBase(rhs) {}
-
-Command& Command::operator=(const Command& rhs) {
-  ErrorBase::operator=(rhs);
-  m_isGrouped = false;
-  return *this;
-}
-
-void Command::Initialize() {}
-void Command::Execute() {}
-void Command::End(bool interrupted) {}
-
-ParallelRaceGroup Command::WithTimeout(units::second_t duration) && {
-  std::vector<std::unique_ptr<Command>> temp;
-  temp.emplace_back(std::make_unique<WaitCommand>(duration));
-  temp.emplace_back(std::move(*this).TransferOwnership());
-  return ParallelRaceGroup(std::move(temp));
-}
-
-ParallelRaceGroup Command::WithInterrupt(std::function<bool()> condition) && {
-  std::vector<std::unique_ptr<Command>> temp;
-  temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
-  temp.emplace_back(std::move(*this).TransferOwnership());
-  return ParallelRaceGroup(std::move(temp));
-}
-
-SequentialCommandGroup Command::BeforeStarting(std::function<void()> toRun) && {
-  std::vector<std::unique_ptr<Command>> temp;
-  temp.emplace_back(std::make_unique<InstantCommand>(
-      std::move(toRun), std::initializer_list<Subsystem*>{}));
-  temp.emplace_back(std::move(*this).TransferOwnership());
-  return SequentialCommandGroup(std::move(temp));
-}
-
-SequentialCommandGroup Command::AndThen(std::function<void()> toRun) && {
-  std::vector<std::unique_ptr<Command>> temp;
-  temp.emplace_back(std::move(*this).TransferOwnership());
-  temp.emplace_back(std::make_unique<InstantCommand>(
-      std::move(toRun), std::initializer_list<Subsystem*>{}));
-  return SequentialCommandGroup(std::move(temp));
-}
-
-PerpetualCommand Command::Perpetually() && {
-  return PerpetualCommand(std::move(*this).TransferOwnership());
-}
-
-ProxyScheduleCommand Command::AsProxy() { return ProxyScheduleCommand(this); }
-
-void Command::Schedule(bool interruptible) {
-  CommandScheduler::GetInstance().Schedule(interruptible, this);
-}
-
-void Command::Cancel() { CommandScheduler::GetInstance().Cancel(this); }
-
-bool Command::IsScheduled() const {
-  return CommandScheduler::GetInstance().IsScheduled(this);
-}
-
-bool Command::HasRequirement(Subsystem* requirement) const {
-  bool hasRequirement = false;
-  for (auto&& subsystem : GetRequirements()) {
-    hasRequirement |= requirement == subsystem;
-  }
-  return hasRequirement;
-}
-
-std::string Command::GetName() const { return GetTypeName(*this); }
-
-bool Command::IsGrouped() const { return m_isGrouped; }
-
-void Command::SetGrouped(bool grouped) { m_isGrouped = grouped; }
-
-namespace frc2 {
-bool RequirementsDisjoint(Command* first, Command* second) {
-  bool disjoint = true;
-  auto&& requirements = second->GetRequirements();
-  for (auto&& requirement : first->GetRequirements()) {
-    disjoint &= requirements.find(requirement) == requirements.end();
-  }
-  return disjoint;
-}
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp
deleted file mode 100644
index aeba26b..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandBase.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/CommandBase.h"
-
-#include <frc/smartdashboard/SendableBuilder.h>
-#include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/CommandScheduler.h>
-#include <frc2/command/SetUtilities.h>
-
-using namespace frc2;
-
-CommandBase::CommandBase() {
-  frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
-}
-
-void CommandBase::AddRequirements(
-    std::initializer_list<Subsystem*> requirements) {
-  m_requirements.insert(requirements.begin(), requirements.end());
-}
-
-void CommandBase::AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements) {
-  m_requirements.insert(requirements.begin(), requirements.end());
-}
-
-wpi::SmallSet<Subsystem*, 4> CommandBase::GetRequirements() const {
-  return m_requirements;
-}
-
-void CommandBase::SetName(const wpi::Twine& name) {
-  frc::SendableRegistry::GetInstance().SetName(this, name);
-}
-
-std::string CommandBase::GetName() const {
-  return frc::SendableRegistry::GetInstance().GetName(this);
-}
-
-std::string CommandBase::GetSubsystem() const {
-  return frc::SendableRegistry::GetInstance().GetSubsystem(this);
-}
-
-void CommandBase::SetSubsystem(const wpi::Twine& subsystem) {
-  frc::SendableRegistry::GetInstance().SetSubsystem(this, subsystem);
-}
-
-void CommandBase::InitSendable(frc::SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Command");
-  builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr);
-  builder.AddBooleanProperty("running", [this] { return IsScheduled(); },
-                             [this](bool value) {
-                               bool isScheduled = IsScheduled();
-                               if (value && !isScheduled) {
-                                 Schedule();
-                               } else if (!value && isScheduled) {
-                                 Cancel();
-                               }
-                             });
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
deleted file mode 100644
index ba53397..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/CommandGroupBase.h"
-
-#include <set>
-
-#include "frc/WPIErrors.h"
-#include "frc2/command/ParallelCommandGroup.h"
-#include "frc2/command/ParallelDeadlineGroup.h"
-#include "frc2/command/ParallelRaceGroup.h"
-#include "frc2/command/SequentialCommandGroup.h"
-
-using namespace frc2;
-template <typename TMap, typename TKey>
-static bool ContainsKey(const TMap& map, TKey keyToCheck) {
-  return map.find(keyToCheck) != map.end();
-}
-bool CommandGroupBase::RequireUngrouped(Command& command) {
-  if (command.IsGrouped()) {
-    wpi_setGlobalWPIErrorWithContext(
-        CommandIllegalUse,
-        "Commands cannot be added to more than one CommandGroup");
-    return false;
-  } else {
-    return true;
-  }
-}
-
-bool CommandGroupBase::RequireUngrouped(
-    wpi::ArrayRef<std::unique_ptr<Command>> commands) {
-  bool allUngrouped = true;
-  for (auto&& command : commands) {
-    allUngrouped &= !command.get()->IsGrouped();
-  }
-  if (!allUngrouped) {
-    wpi_setGlobalWPIErrorWithContext(
-        CommandIllegalUse,
-        "Commands cannot be added to more than one CommandGroup");
-  }
-  return allUngrouped;
-}
-
-bool CommandGroupBase::RequireUngrouped(
-    std::initializer_list<Command*> commands) {
-  bool allUngrouped = true;
-  for (auto&& command : commands) {
-    allUngrouped &= !command->IsGrouped();
-  }
-  if (!allUngrouped) {
-    wpi_setGlobalWPIErrorWithContext(
-        CommandIllegalUse,
-        "Commands cannot be added to more than one CommandGroup");
-  }
-  return allUngrouped;
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp
deleted file mode 100644
index 464a4a6..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandScheduler.cpp
+++ /dev/null
@@ -1,346 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/CommandScheduler.h"
-
-#include <frc/RobotState.h>
-#include <frc/WPIErrors.h>
-#include <frc/commands/Scheduler.h>
-#include <frc/smartdashboard/SendableBuilder.h>
-#include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/CommandGroupBase.h>
-#include <frc2/command/Subsystem.h>
-
-#include <hal/HAL.h>
-
-using namespace frc2;
-template <typename TMap, typename TKey>
-static bool ContainsKey(const TMap& map, TKey keyToCheck) {
-  return map.find(keyToCheck) != map.end();
-}
-
-CommandScheduler::CommandScheduler() {
-  frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
-}
-
-CommandScheduler& CommandScheduler::GetInstance() {
-  static CommandScheduler scheduler;
-  return scheduler;
-}
-
-void CommandScheduler::AddButton(wpi::unique_function<void()> button) {
-  m_buttons.emplace_back(std::move(button));
-}
-
-void CommandScheduler::ClearButtons() { m_buttons.clear(); }
-
-void CommandScheduler::Schedule(bool interruptible, Command* command) {
-  if (m_inRunLoop) {
-    m_toSchedule.try_emplace(command, interruptible);
-    return;
-  }
-
-  if (command->IsGrouped()) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "A command that is part of a command group "
-                               "cannot be independently scheduled");
-    return;
-  }
-  if (m_disabled ||
-      (frc::RobotState::IsDisabled() && !command->RunsWhenDisabled()) ||
-      ContainsKey(m_scheduledCommands, command)) {
-    return;
-  }
-
-  const auto& requirements = command->GetRequirements();
-
-  wpi::SmallVector<Command*, 8> intersection;
-
-  bool isDisjoint = true;
-  bool allInterruptible = true;
-  for (auto&& i1 : m_requirements) {
-    if (requirements.find(i1.first) != requirements.end()) {
-      isDisjoint = false;
-      allInterruptible &= m_scheduledCommands[i1.second].IsInterruptible();
-      intersection.emplace_back(i1.second);
-    }
-  }
-
-  if (isDisjoint || allInterruptible) {
-    if (allInterruptible) {
-      for (auto&& cmdToCancel : intersection) {
-        Cancel(cmdToCancel);
-      }
-    }
-    command->Initialize();
-    m_scheduledCommands[command] = CommandState{interruptible};
-    for (auto&& action : m_initActions) {
-      action(*command);
-    }
-    for (auto&& requirement : requirements) {
-      m_requirements[requirement] = command;
-    }
-  }
-}
-
-void CommandScheduler::Schedule(Command* command) { Schedule(true, command); }
-
-void CommandScheduler::Schedule(bool interruptible,
-                                wpi::ArrayRef<Command*> commands) {
-  for (auto command : commands) {
-    Schedule(interruptible, command);
-  }
-}
-
-void CommandScheduler::Schedule(bool interruptible,
-                                std::initializer_list<Command*> commands) {
-  for (auto command : commands) {
-    Schedule(interruptible, command);
-  }
-}
-
-void CommandScheduler::Schedule(wpi::ArrayRef<Command*> commands) {
-  for (auto command : commands) {
-    Schedule(true, command);
-  }
-}
-
-void CommandScheduler::Schedule(std::initializer_list<Command*> commands) {
-  for (auto command : commands) {
-    Schedule(true, command);
-  }
-}
-
-void CommandScheduler::Run() {
-  if (m_disabled) {
-    return;
-  }
-
-  // Run the periodic method of all registered subsystems.
-  for (auto&& subsystem : m_subsystems) {
-    subsystem.getFirst()->Periodic();
-  }
-
-  // Poll buttons for new commands to add.
-  for (auto&& button : m_buttons) {
-    button();
-  }
-
-  m_inRunLoop = true;
-  // Run scheduled commands, remove finished commands.
-  for (auto iterator = m_scheduledCommands.begin();
-       iterator != m_scheduledCommands.end(); iterator++) {
-    Command* command = iterator->getFirst();
-
-    if (!command->RunsWhenDisabled() && frc::RobotState::IsDisabled()) {
-      Cancel(command);
-      continue;
-    }
-
-    command->Execute();
-    for (auto&& action : m_executeActions) {
-      action(*command);
-    }
-
-    if (command->IsFinished()) {
-      command->End(false);
-      for (auto&& action : m_finishActions) {
-        action(*command);
-      }
-
-      for (auto&& requirement : command->GetRequirements()) {
-        m_requirements.erase(requirement);
-      }
-
-      m_scheduledCommands.erase(iterator);
-    }
-  }
-  m_inRunLoop = false;
-
-  for (auto&& commandInterruptible : m_toSchedule) {
-    Schedule(commandInterruptible.second, commandInterruptible.first);
-  }
-
-  for (auto&& command : m_toCancel) {
-    Cancel(command);
-  }
-
-  m_toSchedule.clear();
-  m_toCancel.clear();
-
-  // Add default commands for un-required registered subsystems.
-  for (auto&& subsystem : m_subsystems) {
-    auto s = m_requirements.find(subsystem.getFirst());
-    if (s == m_requirements.end()) {
-      Schedule({subsystem.getSecond().get()});
-    }
-  }
-}
-
-void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
-  m_subsystems[subsystem] = nullptr;
-}
-
-void CommandScheduler::UnregisterSubsystem(Subsystem* subsystem) {
-  auto s = m_subsystems.find(subsystem);
-  if (s != m_subsystems.end()) {
-    m_subsystems.erase(s);
-  }
-}
-
-void CommandScheduler::RegisterSubsystem(
-    std::initializer_list<Subsystem*> subsystems) {
-  for (auto* subsystem : subsystems) {
-    RegisterSubsystem(subsystem);
-  }
-}
-
-void CommandScheduler::UnregisterSubsystem(
-    std::initializer_list<Subsystem*> subsystems) {
-  for (auto* subsystem : subsystems) {
-    UnregisterSubsystem(subsystem);
-  }
-}
-
-Command* CommandScheduler::GetDefaultCommand(const Subsystem* subsystem) const {
-  auto&& find = m_subsystems.find(subsystem);
-  if (find != m_subsystems.end()) {
-    return find->second.get();
-  } else {
-    return nullptr;
-  }
-}
-
-void CommandScheduler::Cancel(Command* command) {
-  if (m_inRunLoop) {
-    m_toCancel.emplace_back(command);
-    return;
-  }
-
-  auto find = m_scheduledCommands.find(command);
-  if (find == m_scheduledCommands.end()) return;
-  command->End(true);
-  for (auto&& action : m_interruptActions) {
-    action(*command);
-  }
-  m_scheduledCommands.erase(find);
-  for (auto&& requirement : m_requirements) {
-    if (requirement.second == command) {
-      m_requirements.erase(requirement.first);
-    }
-  }
-}
-
-void CommandScheduler::Cancel(wpi::ArrayRef<Command*> commands) {
-  for (auto command : commands) {
-    Cancel(command);
-  }
-}
-
-void CommandScheduler::Cancel(std::initializer_list<Command*> commands) {
-  for (auto command : commands) {
-    Cancel(command);
-  }
-}
-
-void CommandScheduler::CancelAll() {
-  for (auto&& command : m_scheduledCommands) {
-    Cancel(command.first);
-  }
-}
-
-double CommandScheduler::TimeSinceScheduled(const Command* command) const {
-  auto find = m_scheduledCommands.find(command);
-  if (find != m_scheduledCommands.end()) {
-    return find->second.TimeSinceInitialized();
-  } else {
-    return -1;
-  }
-}
-bool CommandScheduler::IsScheduled(
-    wpi::ArrayRef<const Command*> commands) const {
-  for (auto command : commands) {
-    if (!IsScheduled(command)) {
-      return false;
-    }
-  }
-  return true;
-}
-
-bool CommandScheduler::IsScheduled(
-    std::initializer_list<const Command*> commands) const {
-  for (auto command : commands) {
-    if (!IsScheduled(command)) {
-      return false;
-    }
-  }
-  return true;
-}
-
-bool CommandScheduler::IsScheduled(const Command* command) const {
-  return m_scheduledCommands.find(command) != m_scheduledCommands.end();
-}
-
-Command* CommandScheduler::Requiring(const Subsystem* subsystem) const {
-  auto find = m_requirements.find(subsystem);
-  if (find != m_requirements.end()) {
-    return find->second;
-  } else {
-    return nullptr;
-  }
-}
-
-void CommandScheduler::Disable() { m_disabled = true; }
-
-void CommandScheduler::Enable() { m_disabled = false; }
-
-void CommandScheduler::OnCommandInitialize(Action action) {
-  m_initActions.emplace_back(std::move(action));
-}
-
-void CommandScheduler::OnCommandExecute(Action action) {
-  m_executeActions.emplace_back(std::move(action));
-}
-
-void CommandScheduler::OnCommandInterrupt(Action action) {
-  m_interruptActions.emplace_back(std::move(action));
-}
-
-void CommandScheduler::OnCommandFinish(Action action) {
-  m_finishActions.emplace_back(std::move(action));
-}
-
-void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Scheduler");
-  m_namesEntry = builder.GetEntry("Names");
-  m_idsEntry = builder.GetEntry("Ids");
-  m_cancelEntry = builder.GetEntry("Cancel");
-
-  builder.SetUpdateTable([this] {
-    double tmp[1];
-    tmp[0] = 0;
-    auto toCancel = m_cancelEntry.GetDoubleArray(tmp);
-    for (auto cancel : toCancel) {
-      uintptr_t ptrTmp = static_cast<uintptr_t>(cancel);
-      Command* command = reinterpret_cast<Command*>(ptrTmp);
-      if (m_scheduledCommands.find(command) != m_scheduledCommands.end()) {
-        Cancel(command);
-      }
-      m_cancelEntry.SetDoubleArray(wpi::ArrayRef<double>{});
-    }
-
-    wpi::SmallVector<std::string, 8> names;
-    wpi::SmallVector<double, 8> ids;
-    for (auto&& command : m_scheduledCommands) {
-      names.emplace_back(command.first->GetName());
-      uintptr_t ptrTmp = reinterpret_cast<uintptr_t>(command.first);
-      ids.emplace_back(static_cast<double>(ptrTmp));
-    }
-    m_namesEntry.SetStringArray(names);
-    m_idsEntry.SetDoubleArray(ids);
-  });
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp b/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp
deleted file mode 100644
index 78ae006..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/CommandState.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/CommandState.h"
-
-#include "frc/Timer.h"
-
-using namespace frc2;
-CommandState::CommandState(bool interruptible)
-    : m_interruptible{interruptible} {
-  StartTiming();
-  StartRunning();
-}
-
-void CommandState::StartTiming() {
-  m_startTime = frc::Timer::GetFPGATimestamp();
-}
-void CommandState::StartRunning() { m_startTime = -1; }
-double CommandState::TimeSinceInitialized() const {
-  return m_startTime != -1 ? frc::Timer::GetFPGATimestamp() - m_startTime : -1;
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
deleted file mode 100644
index 2344513..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ConditionalCommand.h"
-
-using namespace frc2;
-
-ConditionalCommand::ConditionalCommand(std::unique_ptr<Command>&& onTrue,
-                                       std::unique_ptr<Command>&& onFalse,
-                                       std::function<bool()> condition)
-    : m_condition{std::move(condition)} {
-  if (!CommandGroupBase::RequireUngrouped({onTrue.get(), onFalse.get()})) {
-    return;
-  }
-
-  m_onTrue = std::move(onTrue);
-  m_onFalse = std::move(onFalse);
-
-  m_onTrue->SetGrouped(true);
-  m_onFalse->SetGrouped(true);
-
-  m_runsWhenDisabled &= m_onTrue->RunsWhenDisabled();
-  m_runsWhenDisabled &= m_onFalse->RunsWhenDisabled();
-
-  AddRequirements(m_onTrue->GetRequirements());
-  AddRequirements(m_onFalse->GetRequirements());
-}
-
-void ConditionalCommand::Initialize() {
-  if (m_condition()) {
-    m_selectedCommand = m_onTrue.get();
-  } else {
-    m_selectedCommand = m_onFalse.get();
-  }
-  m_selectedCommand->Initialize();
-}
-
-void ConditionalCommand::Execute() { m_selectedCommand->Execute(); }
-
-void ConditionalCommand::End(bool interrupted) {
-  m_selectedCommand->End(interrupted);
-}
-
-bool ConditionalCommand::IsFinished() {
-  return m_selectedCommand->IsFinished();
-}
-
-bool ConditionalCommand::RunsWhenDisabled() const { return m_runsWhenDisabled; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
deleted file mode 100644
index 63c3179..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/FunctionalCommand.h"
-
-using namespace frc2;
-
-FunctionalCommand::FunctionalCommand(std::function<void()> onInit,
-                                     std::function<void()> onExecute,
-                                     std::function<void(bool)> onEnd,
-                                     std::function<bool()> isFinished)
-    : m_onInit{std::move(onInit)},
-      m_onExecute{std::move(onExecute)},
-      m_onEnd{std::move(onEnd)},
-      m_isFinished{std::move(isFinished)} {}
-
-void FunctionalCommand::Initialize() { m_onInit(); }
-
-void FunctionalCommand::Execute() { m_onExecute(); }
-
-void FunctionalCommand::End(bool interrupted) { m_onEnd(interrupted); }
-
-bool FunctionalCommand::IsFinished() { return m_isFinished(); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp
deleted file mode 100644
index b199074..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/InstantCommand.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/InstantCommand.h"
-
-using namespace frc2;
-
-InstantCommand::InstantCommand(std::function<void()> toRun,
-                               std::initializer_list<Subsystem*> requirements)
-    : m_toRun{std::move(toRun)} {
-  AddRequirements(requirements);
-}
-
-InstantCommand::InstantCommand() : m_toRun{[] {}} {}
-
-void InstantCommand::Initialize() { m_toRun(); }
-
-bool InstantCommand::IsFinished() { return true; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp
deleted file mode 100644
index c4eac81..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/NotifierCommand.cpp
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/NotifierCommand.h"
-
-using namespace frc2;
-
-NotifierCommand::NotifierCommand(std::function<void()> toRun,
-                                 units::second_t period,
-                                 std::initializer_list<Subsystem*> requirements)
-    : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} {
-  AddRequirements(requirements);
-}
-
-NotifierCommand::NotifierCommand(NotifierCommand&& other)
-    : CommandHelper(std::move(other)),
-      m_toRun(other.m_toRun),
-      m_notifier(other.m_toRun),
-      m_period(other.m_period) {}
-
-NotifierCommand::NotifierCommand(const NotifierCommand& other)
-    : CommandHelper(other),
-      m_toRun(other.m_toRun),
-      m_notifier(frc::Notifier(other.m_toRun)),
-      m_period(other.m_period) {}
-
-void NotifierCommand::Initialize() { m_notifier.StartPeriodic(m_period); }
-
-void NotifierCommand::End(bool interrupted) { m_notifier.Stop(); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp
deleted file mode 100644
index 4391e60..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/PIDCommand.cpp
+++ /dev/null
@@ -1,59 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/PIDCommand.h"
-
-using namespace frc2;
-
-PIDCommand::PIDCommand(PIDController controller,
-                       std::function<double()> measurementSource,
-                       std::function<double()> setpointSource,
-                       std::function<void(double)> useOutput,
-                       std::initializer_list<Subsystem*> requirements)
-    : m_controller{controller},
-      m_measurement{std::move(measurementSource)},
-      m_setpoint{std::move(setpointSource)},
-      m_useOutput{std::move(useOutput)} {
-  AddRequirements(requirements);
-}
-
-PIDCommand::PIDCommand(PIDController controller,
-                       std::function<double()> measurementSource,
-                       double setpoint, std::function<void(double)> useOutput,
-                       std::initializer_list<Subsystem*> requirements)
-    : PIDCommand(controller, measurementSource, [setpoint] { return setpoint; },
-                 useOutput, requirements) {}
-
-void PIDCommand::Initialize() { m_controller.Reset(); }
-
-void PIDCommand::Execute() {
-  UseOutput(m_controller.Calculate(GetMeasurement(), m_setpoint()));
-}
-
-void PIDCommand::End(bool interrupted) { UseOutput(0); }
-
-void PIDCommand::SetOutput(std::function<void(double)> useOutput) {
-  m_useOutput = useOutput;
-}
-
-void PIDCommand::SetSetpoint(std::function<double()> setpointSource) {
-  m_setpoint = setpointSource;
-}
-
-void PIDCommand::SetSetpoint(double setpoint) {
-  m_setpoint = [setpoint] { return setpoint; };
-}
-
-void PIDCommand::SetSetpointRelative(double relativeSetpoint) {
-  SetSetpoint(m_setpoint() + relativeSetpoint);
-}
-
-double PIDCommand::GetMeasurement() { return m_measurement(); }
-
-void PIDCommand::UseOutput(double output) { m_useOutput(output); }
-
-PIDController& PIDCommand::getController() { return m_controller; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
deleted file mode 100644
index b81f6f6..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
+++ /dev/null
@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/PIDSubsystem.h"
-
-using namespace frc2;
-
-PIDSubsystem::PIDSubsystem(PIDController controller)
-    : m_controller{controller} {}
-
-void PIDSubsystem::Periodic() {
-  if (m_enabled) {
-    UseOutput(m_controller.Calculate(GetMeasurement(), GetSetpoint()));
-  }
-}
-
-void PIDSubsystem::Enable() {
-  m_controller.Reset();
-  m_enabled = true;
-}
-
-void PIDSubsystem::Disable() {
-  UseOutput(0);
-  m_enabled = false;
-}
-
-PIDController& PIDSubsystem::GetController() { return m_controller; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
deleted file mode 100644
index d8a4159..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
+++ /dev/null
@@ -1,83 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ParallelCommandGroup.h"
-
-using namespace frc2;
-
-ParallelCommandGroup::ParallelCommandGroup(
-    std::vector<std::unique_ptr<Command>>&& commands) {
-  AddCommands(std::move(commands));
-}
-
-void ParallelCommandGroup::Initialize() {
-  for (auto& commandRunning : m_commands) {
-    commandRunning.first->Initialize();
-    commandRunning.second = true;
-  }
-  isRunning = true;
-}
-
-void ParallelCommandGroup::Execute() {
-  for (auto& commandRunning : m_commands) {
-    if (!commandRunning.second) continue;
-    commandRunning.first->Execute();
-    if (commandRunning.first->IsFinished()) {
-      commandRunning.first->End(false);
-      commandRunning.second = false;
-    }
-  }
-}
-
-void ParallelCommandGroup::End(bool interrupted) {
-  if (interrupted) {
-    for (auto& commandRunning : m_commands) {
-      if (commandRunning.second) {
-        commandRunning.first->End(true);
-      }
-    }
-  }
-  isRunning = false;
-}
-
-bool ParallelCommandGroup::IsFinished() {
-  for (auto& command : m_commands) {
-    if (command.second) return false;
-  }
-  return true;
-}
-
-bool ParallelCommandGroup::RunsWhenDisabled() const {
-  return m_runWhenDisabled;
-}
-
-void ParallelCommandGroup::AddCommands(
-    std::vector<std::unique_ptr<Command>>&& commands) {
-  for (auto&& command : commands) {
-    if (!RequireUngrouped(*command)) return;
-  }
-
-  if (isRunning) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "Commands cannot be added to a CommandGroup "
-                               "while the group is running");
-  }
-
-  for (auto&& command : commands) {
-    if (RequirementsDisjoint(this, command.get())) {
-      command->SetGrouped(true);
-      AddRequirements(command->GetRequirements());
-      m_runWhenDisabled &= command->RunsWhenDisabled();
-      m_commands[std::move(command)] = false;
-    } else {
-      wpi_setWPIErrorWithContext(CommandIllegalUse,
-                                 "Multiple commands in a parallel group cannot "
-                                 "require the same subsystems");
-      return;
-    }
-  }
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
deleted file mode 100644
index d967e67..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ParallelDeadlineGroup.h"
-
-using namespace frc2;
-
-ParallelDeadlineGroup::ParallelDeadlineGroup(
-    std::unique_ptr<Command>&& deadline,
-    std::vector<std::unique_ptr<Command>>&& commands) {
-  SetDeadline(std::move(deadline));
-  AddCommands(std::move(commands));
-}
-
-void ParallelDeadlineGroup::Initialize() {
-  for (auto& commandRunning : m_commands) {
-    commandRunning.first->Initialize();
-    commandRunning.second = true;
-  }
-  isRunning = true;
-}
-
-void ParallelDeadlineGroup::Execute() {
-  for (auto& commandRunning : m_commands) {
-    if (!commandRunning.second) continue;
-    commandRunning.first->Execute();
-    if (commandRunning.first->IsFinished()) {
-      commandRunning.first->End(false);
-      commandRunning.second = false;
-    }
-  }
-}
-
-void ParallelDeadlineGroup::End(bool interrupted) {
-  for (auto& commandRunning : m_commands) {
-    if (commandRunning.second) {
-      commandRunning.first->End(true);
-    }
-  }
-  isRunning = false;
-}
-
-bool ParallelDeadlineGroup::IsFinished() { return m_deadline->IsFinished(); }
-
-bool ParallelDeadlineGroup::RunsWhenDisabled() const {
-  return m_runWhenDisabled;
-}
-
-void ParallelDeadlineGroup::AddCommands(
-    std::vector<std::unique_ptr<Command>>&& commands) {
-  if (!RequireUngrouped(commands)) {
-    return;
-  }
-
-  if (isRunning) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "Commands cannot be added to a CommandGroup "
-                               "while the group is running");
-  }
-
-  for (auto&& command : commands) {
-    if (RequirementsDisjoint(this, command.get())) {
-      command->SetGrouped(true);
-      AddRequirements(command->GetRequirements());
-      m_runWhenDisabled &= command->RunsWhenDisabled();
-      m_commands[std::move(command)] = false;
-    } else {
-      wpi_setWPIErrorWithContext(CommandIllegalUse,
-                                 "Multiple commands in a parallel group cannot "
-                                 "require the same subsystems");
-      return;
-    }
-  }
-}
-
-void ParallelDeadlineGroup::SetDeadline(std::unique_ptr<Command>&& deadline) {
-  m_deadline = deadline.get();
-  m_deadline->SetGrouped(true);
-  m_commands[std::move(deadline)] = false;
-  AddRequirements(m_deadline->GetRequirements());
-  m_runWhenDisabled &= m_deadline->RunsWhenDisabled();
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
deleted file mode 100644
index 8a02717..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ParallelRaceGroup.h"
-
-using namespace frc2;
-
-ParallelRaceGroup::ParallelRaceGroup(
-    std::vector<std::unique_ptr<Command>>&& commands) {
-  AddCommands(std::move(commands));
-}
-
-void ParallelRaceGroup::Initialize() {
-  for (auto& commandRunning : m_commands) {
-    commandRunning->Initialize();
-  }
-  isRunning = true;
-}
-
-void ParallelRaceGroup::Execute() {
-  for (auto& commandRunning : m_commands) {
-    commandRunning->Execute();
-    if (commandRunning->IsFinished()) {
-      m_finished = true;
-    }
-  }
-}
-
-void ParallelRaceGroup::End(bool interrupted) {
-  for (auto& commandRunning : m_commands) {
-    commandRunning->End(!commandRunning->IsFinished());
-  }
-  isRunning = false;
-}
-
-bool ParallelRaceGroup::IsFinished() { return m_finished; }
-
-bool ParallelRaceGroup::RunsWhenDisabled() const { return m_runWhenDisabled; }
-
-void ParallelRaceGroup::AddCommands(
-    std::vector<std::unique_ptr<Command>>&& commands) {
-  if (!RequireUngrouped(commands)) {
-    return;
-  }
-
-  if (isRunning) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "Commands cannot be added to a CommandGroup "
-                               "while the group is running");
-  }
-
-  for (auto&& command : commands) {
-    if (RequirementsDisjoint(this, command.get())) {
-      command->SetGrouped(true);
-      AddRequirements(command->GetRequirements());
-      m_runWhenDisabled &= command->RunsWhenDisabled();
-      m_commands.emplace(std::move(command));
-    } else {
-      wpi_setWPIErrorWithContext(CommandIllegalUse,
-                                 "Multiple commands in a parallel group cannot "
-                                 "require the same subsystems");
-      return;
-    }
-  }
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
deleted file mode 100644
index f29850b..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/PerpetualCommand.h"
-
-using namespace frc2;
-
-PerpetualCommand::PerpetualCommand(std::unique_ptr<Command>&& command) {
-  if (!CommandGroupBase::RequireUngrouped(command)) {
-    return;
-  }
-  m_command = std::move(command);
-  m_command->SetGrouped(true);
-  AddRequirements(m_command->GetRequirements());
-}
-
-void PerpetualCommand::Initialize() { m_command->Initialize(); }
-
-void PerpetualCommand::Execute() { m_command->Execute(); }
-
-void PerpetualCommand::End(bool interrupted) { m_command->End(interrupted); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
deleted file mode 100644
index c0cc19b..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDCommand.cpp
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ProfiledPIDCommand.h"
-
-using namespace frc2;
-using State = frc::TrapezoidProfile::State;
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource,
-    std::function<State()> goalSource,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : m_controller{controller},
-      m_measurement{std::move(measurementSource)},
-      m_goal{std::move(goalSource)},
-      m_useOutput{std::move(useOutput)} {
-  AddRequirements(requirements);
-}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource,
-    std::function<units::meter_t()> goalSource,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : ProfiledPIDCommand(controller, measurementSource,
-                         [&goalSource]() {
-                           return State{goalSource(), 0_mps};
-                         },
-                         useOutput, requirements) {}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource, State goal,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
-                         useOutput, requirements) {}
-
-ProfiledPIDCommand::ProfiledPIDCommand(
-    frc::ProfiledPIDController controller,
-    std::function<units::meter_t()> measurementSource, units::meter_t goal,
-    std::function<void(double, State)> useOutput,
-    std::initializer_list<Subsystem*> requirements)
-    : ProfiledPIDCommand(controller, measurementSource, [goal] { return goal; },
-                         useOutput, requirements) {}
-
-void ProfiledPIDCommand::Initialize() { m_controller.Reset(); }
-
-void ProfiledPIDCommand::Execute() {
-  UseOutput(m_controller.Calculate(GetMeasurement(), m_goal()),
-            m_controller.GetSetpoint());
-}
-
-void ProfiledPIDCommand::End(bool interrupted) {
-  UseOutput(0, State{0_m, 0_mps});
-}
-
-units::meter_t ProfiledPIDCommand::GetMeasurement() { return m_measurement(); }
-
-void ProfiledPIDCommand::UseOutput(double output, State state) {
-  m_useOutput(output, state);
-}
-
-frc::ProfiledPIDController& ProfiledPIDCommand::GetController() {
-  return m_controller;
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp
deleted file mode 100644
index 469e153..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ProfiledPIDSubsystem.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ProfiledPIDSubsystem.h"
-
-using namespace frc2;
-using State = frc::TrapezoidProfile::State;
-
-ProfiledPIDSubsystem::ProfiledPIDSubsystem(
-    frc::ProfiledPIDController controller)
-    : m_controller{controller} {}
-
-void ProfiledPIDSubsystem::Periodic() {
-  if (m_enabled) {
-    UseOutput(m_controller.Calculate(GetMeasurement(), GetGoal()),
-              m_controller.GetSetpoint());
-  }
-}
-
-void ProfiledPIDSubsystem::Enable() {
-  m_controller.Reset();
-  m_enabled = true;
-}
-
-void ProfiledPIDSubsystem::Disable() {
-  UseOutput(0, State{0_m, 0_mps});
-  m_enabled = false;
-}
-
-frc::ProfiledPIDController& ProfiledPIDSubsystem::GetController() {
-  return m_controller;
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
deleted file mode 100644
index 6f96315..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ProxyScheduleCommand.h"
-
-using namespace frc2;
-
-ProxyScheduleCommand::ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
-  SetInsert(m_toSchedule, toSchedule);
-}
-
-void ProxyScheduleCommand::Initialize() {
-  for (auto* command : m_toSchedule) {
-    command->Schedule();
-  }
-}
-
-void ProxyScheduleCommand::End(bool interrupted) {
-  if (interrupted) {
-    for (auto* command : m_toSchedule) {
-      command->Cancel();
-    }
-  }
-}
-
-void ProxyScheduleCommand::Execute() {
-  m_finished = true;
-  for (auto* command : m_toSchedule) {
-    m_finished &= !command->IsScheduled();
-  }
-}
-
-bool ProxyScheduleCommand::IsFinished() { return m_finished; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp
deleted file mode 100644
index 0de739e..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/RamseteCommand.cpp
+++ /dev/null
@@ -1,113 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/RamseteCommand.h"
-
-using namespace frc2;
-using namespace units;
-
-template <typename T>
-int sgn(T val) {
-  return (T(0) < val) - (val < T(0));
-}
-
-RamseteCommand::RamseteCommand(
-    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
-    frc::RamseteController controller, volt_t ks,
-    units::unit_t<voltsecondspermeter> kv,
-    units::unit_t<voltsecondssquaredpermeter> ka,
-    frc::DifferentialDriveKinematics kinematics,
-    std::function<units::meters_per_second_t()> leftSpeed,
-    std::function<units::meters_per_second_t()> rightSpeed,
-    frc2::PIDController leftController, frc2::PIDController rightController,
-    std::function<void(volt_t, volt_t)> output,
-    std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
-      m_controller(controller),
-      m_ks(ks),
-      m_kv(kv),
-      m_ka(ka),
-      m_kinematics(kinematics),
-      m_leftSpeed(leftSpeed),
-      m_rightSpeed(rightSpeed),
-      m_leftController(std::make_unique<frc2::PIDController>(leftController)),
-      m_rightController(std::make_unique<frc2::PIDController>(rightController)),
-      m_outputVolts(output) {
-  AddRequirements(requirements);
-}
-
-RamseteCommand::RamseteCommand(
-    frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
-    frc::RamseteController controller,
-    frc::DifferentialDriveKinematics kinematics,
-    std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
-        output,
-    std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
-      m_controller(controller),
-      m_ks(0),
-      m_kv(0),
-      m_ka(0),
-      m_kinematics(kinematics),
-      m_outputVel(output) {
-  AddRequirements(requirements);
-}
-
-void RamseteCommand::Initialize() {
-  m_prevTime = 0_s;
-  auto initialState = m_trajectory.Sample(0_s);
-  m_prevSpeeds = m_kinematics.ToWheelSpeeds(
-      frc::ChassisSpeeds{initialState.velocity, 0_mps,
-                         initialState.velocity * initialState.curvature});
-  m_timer.Reset();
-  m_timer.Start();
-  m_leftController->Reset();
-  m_rightController->Reset();
-}
-
-void RamseteCommand::Execute() {
-  auto curTime = m_timer.Get();
-  auto dt = curTime - m_prevTime;
-
-  auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
-      m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
-
-  if (m_leftController.get() != nullptr) {
-    auto leftFeedforward =
-        m_ks * sgn(targetWheelSpeeds.left) + m_kv * targetWheelSpeeds.left +
-        m_ka * (targetWheelSpeeds.left - m_prevSpeeds.left) / dt;
-
-    auto rightFeedforward =
-        m_ks * sgn(targetWheelSpeeds.right) + m_kv * targetWheelSpeeds.right +
-        m_ka * (targetWheelSpeeds.right - m_prevSpeeds.right) / dt;
-
-    auto leftOutput =
-        volt_t(m_leftController->Calculate(
-            m_leftSpeed().to<double>(), targetWheelSpeeds.left.to<double>())) +
-        leftFeedforward;
-
-    auto rightOutput = volt_t(m_rightController->Calculate(
-                           m_rightSpeed().to<double>(),
-                           targetWheelSpeeds.right.to<double>())) +
-                       rightFeedforward;
-
-    m_outputVolts(leftOutput, rightOutput);
-  } else {
-    m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right);
-  }
-
-  m_prevTime = curTime;
-  m_prevSpeeds = targetWheelSpeeds;
-}
-
-void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }
-
-bool RamseteCommand::IsFinished() {
-  return m_timer.HasPeriodPassed(m_trajectory.TotalTime());
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp
deleted file mode 100644
index 5c2c755..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/RunCommand.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/RunCommand.h"
-
-using namespace frc2;
-
-RunCommand::RunCommand(std::function<void()> toRun,
-                       std::initializer_list<Subsystem*> requirements)
-    : m_toRun{std::move(toRun)} {
-  AddRequirements(requirements);
-}
-
-void RunCommand::Execute() { m_toRun(); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
deleted file mode 100644
index ea1ea8d..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/ScheduleCommand.h"
-
-using namespace frc2;
-
-ScheduleCommand::ScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
-  SetInsert(m_toSchedule, toSchedule);
-}
-
-void ScheduleCommand::Initialize() {
-  for (auto command : m_toSchedule) {
-    command->Schedule();
-  }
-}
-
-bool ScheduleCommand::IsFinished() { return true; }
-
-bool ScheduleCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
deleted file mode 100644
index 1aa19e4..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/SequentialCommandGroup.h"
-
-using namespace frc2;
-
-SequentialCommandGroup::SequentialCommandGroup(
-    std::vector<std::unique_ptr<Command>>&& commands) {
-  AddCommands(std::move(commands));
-}
-
-void SequentialCommandGroup::Initialize() {
-  m_currentCommandIndex = 0;
-
-  if (!m_commands.empty()) {
-    m_commands[0]->Initialize();
-  }
-}
-
-void SequentialCommandGroup::Execute() {
-  if (m_commands.empty()) return;
-
-  auto& currentCommand = m_commands[m_currentCommandIndex];
-
-  currentCommand->Execute();
-  if (currentCommand->IsFinished()) {
-    currentCommand->End(false);
-    m_currentCommandIndex++;
-    if (m_currentCommandIndex < m_commands.size()) {
-      m_commands[m_currentCommandIndex]->Initialize();
-    }
-  }
-}
-
-void SequentialCommandGroup::End(bool interrupted) {
-  if (interrupted && !m_commands.empty() &&
-      m_currentCommandIndex != invalid_index &&
-      m_currentCommandIndex < m_commands.size()) {
-    m_commands[m_currentCommandIndex]->End(interrupted);
-  }
-  m_currentCommandIndex = invalid_index;
-}
-
-bool SequentialCommandGroup::IsFinished() {
-  return m_currentCommandIndex == m_commands.size();
-}
-
-bool SequentialCommandGroup::RunsWhenDisabled() const {
-  return m_runWhenDisabled;
-}
-
-void SequentialCommandGroup::AddCommands(
-    std::vector<std::unique_ptr<Command>>&& commands) {
-  if (!RequireUngrouped(commands)) {
-    return;
-  }
-
-  if (m_currentCommandIndex != invalid_index) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "Commands cannot be added to a CommandGroup "
-                               "while the group is running");
-  }
-
-  for (auto&& command : commands) {
-    command->SetGrouped(true);
-    AddRequirements(command->GetRequirements());
-    m_runWhenDisabled &= command->RunsWhenDisabled();
-    m_commands.emplace_back(std::move(command));
-  }
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp
deleted file mode 100644
index 33407bf..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/StartEndCommand.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/StartEndCommand.h"
-
-using namespace frc2;
-
-StartEndCommand::StartEndCommand(std::function<void()> onInit,
-                                 std::function<void()> onEnd,
-                                 std::initializer_list<Subsystem*> requirements)
-    : m_onInit{std::move(onInit)}, m_onEnd{std::move(onEnd)} {
-  AddRequirements(requirements);
-}
-
-StartEndCommand::StartEndCommand(const StartEndCommand& other)
-    : CommandHelper(other) {
-  m_onInit = other.m_onInit;
-  m_onEnd = other.m_onEnd;
-}
-
-void StartEndCommand::Initialize() { m_onInit(); }
-
-void StartEndCommand::End(bool interrupted) { m_onEnd(); }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp b/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp
deleted file mode 100644
index cccb55b..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/Subsystem.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/Subsystem.h"
-
-using namespace frc2;
-Subsystem::~Subsystem() {
-  CommandScheduler::GetInstance().UnregisterSubsystem(this);
-}
-
-void Subsystem::Periodic() {}
-
-Command* Subsystem::GetDefaultCommand() const {
-  return CommandScheduler::GetInstance().GetDefaultCommand(this);
-}
-
-Command* Subsystem::GetCurrentCommand() const {
-  return CommandScheduler::GetInstance().Requiring(this);
-}
-
-void Subsystem::Register() {
-  return CommandScheduler::GetInstance().RegisterSubsystem(this);
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp
deleted file mode 100644
index 226f080..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/SubsystemBase.cpp
+++ /dev/null
@@ -1,66 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/SubsystemBase.h"
-
-#include <frc/smartdashboard/SendableBuilder.h>
-#include <frc/smartdashboard/SendableRegistry.h>
-#include <frc2/command/Command.h>
-#include <frc2/command/CommandScheduler.h>
-
-using namespace frc2;
-
-SubsystemBase::SubsystemBase() {
-  frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
-  CommandScheduler::GetInstance().RegisterSubsystem({this});
-}
-
-void SubsystemBase::InitSendable(frc::SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Subsystem");
-  builder.AddBooleanProperty(".hasDefault",
-                             [this] { return GetDefaultCommand() != nullptr; },
-                             nullptr);
-  builder.AddStringProperty(".default",
-                            [this]() -> std::string {
-                              auto command = GetDefaultCommand();
-                              if (command == nullptr) return "none";
-                              return command->GetName();
-                            },
-                            nullptr);
-  builder.AddBooleanProperty(".hasCommand",
-                             [this] { return GetCurrentCommand() != nullptr; },
-                             nullptr);
-  builder.AddStringProperty(".command",
-                            [this]() -> std::string {
-                              auto command = GetCurrentCommand();
-                              if (command == nullptr) return "none";
-                              return command->GetName();
-                            },
-                            nullptr);
-}
-
-std::string SubsystemBase::GetName() const {
-  return frc::SendableRegistry::GetInstance().GetName(this);
-}
-
-void SubsystemBase::SetName(const wpi::Twine& name) {
-  frc::SendableRegistry::GetInstance().SetName(this, name);
-}
-
-std::string SubsystemBase::GetSubsystem() const {
-  return frc::SendableRegistry::GetInstance().GetSubsystem(this);
-}
-
-void SubsystemBase::SetSubsystem(const wpi::Twine& name) {
-  frc::SendableRegistry::GetInstance().SetSubsystem(this, name);
-}
-
-void SubsystemBase::AddChild(std::string name, frc::Sendable* child) {
-  auto& registry = frc::SendableRegistry::GetInstance();
-  registry.AddLW(child, GetSubsystem(), name);
-  registry.AddChild(this, child);
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
deleted file mode 100644
index bb17edc..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/TrapezoidProfileCommand.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/TrapezoidProfileCommand.h"
-
-#include <units/units.h>
-
-using namespace frc2;
-
-TrapezoidProfileCommand::TrapezoidProfileCommand(
-    frc::TrapezoidProfile profile,
-    std::function<void(frc::TrapezoidProfile::State)> output,
-    std::initializer_list<Subsystem*> requirements)
-    : m_profile(profile), m_output(output) {
-  AddRequirements(requirements);
-}
-
-void TrapezoidProfileCommand::Initialize() {
-  m_timer.Reset();
-  m_timer.Start();
-}
-
-void TrapezoidProfileCommand::Execute() {
-  m_output(m_profile.Calculate(m_timer.Get()));
-}
-
-void TrapezoidProfileCommand::End(bool interrupted) { m_timer.Stop(); }
-
-bool TrapezoidProfileCommand::IsFinished() {
-  return m_timer.HasPeriodPassed(m_profile.TotalTime());
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp
deleted file mode 100644
index 1279d66..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/WaitCommand.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/WaitCommand.h"
-
-using namespace frc2;
-
-WaitCommand::WaitCommand(units::second_t duration) : m_duration{duration} {
-  auto durationStr = std::to_string(duration.to<double>());
-  SetName(wpi::Twine(GetName()) + ": " + wpi::Twine(durationStr) + " seconds");
-}
-
-void WaitCommand::Initialize() {
-  m_timer.Reset();
-  m_timer.Start();
-}
-
-void WaitCommand::End(bool interrupted) { m_timer.Stop(); }
-
-bool WaitCommand::IsFinished() { return m_timer.HasPeriodPassed(m_duration); }
-
-bool WaitCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp b/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
deleted file mode 100644
index d7a0daf..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/WaitUntilCommand.h"
-
-using namespace frc2;
-
-WaitUntilCommand::WaitUntilCommand(std::function<bool()> condition)
-    : m_condition{std::move(condition)} {}
-
-WaitUntilCommand::WaitUntilCommand(double time)
-    : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0; }} {}
-
-bool WaitUntilCommand::IsFinished() { return m_condition(); }
-
-bool WaitUntilCommand::RunsWhenDisabled() const { return true; }
diff --git a/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp b/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp
deleted file mode 100644
index e519a9f..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/button/Button.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/button/Button.h"
-
-using namespace frc2;
-
-Button::Button(std::function<bool()> isPressed) : Trigger(isPressed) {}
-
-Button Button::WhenPressed(Command* command, bool interruptible) {
-  WhenActive(command, interruptible);
-  return *this;
-}
-
-Button Button::WhenPressed(std::function<void()> toRun) {
-  WhenActive(std::move(toRun));
-  return *this;
-}
-
-Button Button::WhileHeld(Command* command, bool interruptible) {
-  WhileActiveContinous(command, interruptible);
-  return *this;
-}
-
-Button Button::WhileHeld(std::function<void()> toRun) {
-  WhileActiveContinous(std::move(toRun));
-  return *this;
-}
-
-Button Button::WhenHeld(Command* command, bool interruptible) {
-  WhileActiveOnce(command, interruptible);
-  return *this;
-}
-
-Button Button::WhenReleased(Command* command, bool interruptible) {
-  WhenInactive(command, interruptible);
-  return *this;
-}
-
-Button Button::WhenReleased(std::function<void()> toRun) {
-  WhenInactive(std::move(toRun));
-  return *this;
-}
-
-Button Button::ToggleWhenPressed(Command* command, bool interruptible) {
-  ToggleWhenActive(command, interruptible);
-  return *this;
-}
-
-Button Button::CancelWhenPressed(Command* command) {
-  CancelWhenActive(command);
-  return *this;
-}
diff --git a/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp b/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp
deleted file mode 100644
index 304bf98..0000000
--- a/wpilibc/src/main/native/cpp/frc2/command/button/Trigger.cpp
+++ /dev/null
@@ -1,119 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/command/button/Trigger.h"
-
-#include <frc2/command/InstantCommand.h>
-
-using namespace frc2;
-
-Trigger::Trigger(const Trigger& other) : m_isActive(other.m_isActive) {}
-
-Trigger Trigger::WhenActive(Command* command, bool interruptible) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
-
-        if (!pressedLast && pressed) {
-          command->Schedule(interruptible);
-        }
-
-        pressedLast = pressed;
-      });
-
-  return *this;
-}
-
-Trigger Trigger::WhenActive(std::function<void()> toRun) {
-  return WhenActive(InstantCommand(std::move(toRun), {}));
-}
-
-Trigger Trigger::WhileActiveContinous(Command* command, bool interruptible) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
-
-        if (pressed) {
-          command->Schedule(interruptible);
-        } else if (pressedLast && !pressed) {
-          command->Cancel();
-        }
-
-        pressedLast = pressed;
-      });
-  return *this;
-}
-
-Trigger Trigger::WhileActiveContinous(std::function<void()> toRun) {
-  return WhileActiveContinous(InstantCommand(std::move(toRun), {}));
-}
-
-Trigger Trigger::WhileActiveOnce(Command* command, bool interruptible) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
-
-        if (!pressedLast && pressed) {
-          command->Schedule(interruptible);
-        } else if (pressedLast && !pressed) {
-          command->Cancel();
-        }
-
-        pressedLast = pressed;
-      });
-  return *this;
-}
-
-Trigger Trigger::WhenInactive(Command* command, bool interruptible) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
-
-        if (pressedLast && !pressed) {
-          command->Schedule(interruptible);
-        }
-
-        pressedLast = pressed;
-      });
-  return *this;
-}
-
-Trigger Trigger::WhenInactive(std::function<void()> toRun) {
-  return WhenInactive(InstantCommand(std::move(toRun), {}));
-}
-
-Trigger Trigger::ToggleWhenActive(Command* command, bool interruptible) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command, interruptible]() mutable {
-        bool pressed = Get();
-
-        if (!pressedLast && pressed) {
-          if (command->IsScheduled()) {
-            command->Cancel();
-          } else {
-            command->Schedule(interruptible);
-          }
-        }
-
-        pressedLast = pressed;
-      });
-  return *this;
-}
-
-Trigger Trigger::CancelWhenActive(Command* command) {
-  CommandScheduler::GetInstance().AddButton(
-      [pressedLast = Get(), *this, command]() mutable {
-        bool pressed = Get();
-
-        if (!pressedLast && pressed) {
-          command->Cancel();
-        }
-
-        pressedLast = pressed;
-      });
-  return *this;
-}
diff --git a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
index 42d20f7..1754830 100644
--- a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
@@ -9,6 +9,8 @@
 
 #include <cmath>
 
+#include <wpi/json.h>
+
 using namespace frc;
 
 Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
@@ -96,3 +98,13 @@
 
   return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)};
 }
+
+void frc::to_json(wpi::json& json, const Pose2d& pose) {
+  json = wpi::json{{"translation", pose.Translation()},
+                   {"rotation", pose.Rotation()}};
+}
+
+void frc::from_json(const wpi::json& json, Pose2d& pose) {
+  pose = Pose2d{json.at("translation").get<Translation2d>(),
+                json.at("rotation").get<Rotation2d>()};
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
index 2723442..a3c4bea 100644
--- a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -9,6 +9,8 @@
 
 #include <cmath>
 
+#include <wpi/json.h>
+
 using namespace frc;
 
 Rotation2d::Rotation2d(units::radian_t value)
@@ -68,3 +70,11 @@
   return {Cos() * other.Cos() - Sin() * other.Sin(),
           Cos() * other.Sin() + Sin() * other.Cos()};
 }
+
+void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
+  json = wpi::json{{"radians", rotation.Radians().to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
+  rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
+}
diff --git a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
index ca287d1..c3db7e3 100644
--- a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
@@ -7,6 +7,8 @@
 
 #include "frc/geometry/Translation2d.h"
 
+#include <wpi/json.h>
+
 using namespace frc;
 
 Translation2d::Translation2d(units::meter_t x, units::meter_t y)
@@ -73,3 +75,13 @@
   *this *= (1.0 / scalar);
   return *this;
 }
+
+void frc::to_json(wpi::json& json, const Translation2d& translation) {
+  json = wpi::json{{"x", translation.X().to<double>()},
+                   {"y", translation.Y().to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Translation2d& translation) {
+  translation = Translation2d{units::meter_t{json.at("x").get<double>()},
+                              units::meter_t{json.at("y").get<double>()}};
+}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp
deleted file mode 100644
index 8301481..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveKinematics.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-
-using namespace frc;
-
-DifferentialDriveKinematics::DifferentialDriveKinematics(
-    units::meter_t trackWidth)
-    : m_trackWidth(trackWidth) {}
-
-ChassisSpeeds DifferentialDriveKinematics::ToChassisSpeeds(
-    const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
-  return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
-          (wheelSpeeds.right - wheelSpeeds.left) / m_trackWidth * 1_rad};
-}
-
-DifferentialDriveWheelSpeeds DifferentialDriveKinematics::ToWheelSpeeds(
-    const ChassisSpeeds& chassisSpeeds) const {
-  return {chassisSpeeds.vx - m_trackWidth / 2 * chassisSpeeds.omega / 1_rad,
-          chassisSpeeds.vx + m_trackWidth / 2 * chassisSpeeds.omega / 1_rad};
-}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index 418dd0f..8591da4 100644
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -10,23 +10,26 @@
 using namespace frc;
 
 DifferentialDriveOdometry::DifferentialDriveOdometry(
-    DifferentialDriveKinematics kinematics, const Pose2d& initialPose)
-    : m_kinematics(kinematics), m_pose(initialPose) {
+    const Rotation2d& gyroAngle, const Pose2d& initialPose)
+    : m_pose(initialPose) {
   m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
 }
 
-const Pose2d& DifferentialDriveOdometry::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& angle,
-    const DifferentialDriveWheelSpeeds& wheelSpeeds) {
-  units::second_t deltaTime =
-      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
-  m_previousTime = currentTime;
+const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle,
+                                                units::meter_t leftDistance,
+                                                units::meter_t rightDistance) {
+  auto deltaLeftDistance = leftDistance - m_prevLeftDistance;
+  auto deltaRightDistance = rightDistance - m_prevRightDistance;
 
-  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
-  static_cast<void>(dtheta);
+  m_prevLeftDistance = leftDistance;
+  m_prevRightDistance = rightDistance;
+
+  auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+  auto angle = gyroAngle + m_gyroOffset;
 
   auto newPose = m_pose.Exp(
-      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+      {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
 
   m_previousAngle = angle;
   m_pose = {newPose.Translation(), angle};
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index 18e5635..615635a 100644
--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -10,18 +10,22 @@
 using namespace frc;
 
 MecanumDriveOdometry::MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+                                           const Rotation2d& gyroAngle,
                                            const Pose2d& initialPose)
     : m_kinematics(kinematics), m_pose(initialPose) {
   m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
 }
 
 const Pose2d& MecanumDriveOdometry::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& angle,
+    units::second_t currentTime, const Rotation2d& gyroAngle,
     MecanumDriveWheelSpeeds wheelSpeeds) {
   units::second_t deltaTime =
       (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
   m_previousTime = currentTime;
 
+  auto angle = gyroAngle + m_gyroOffset;
+
   auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(wheelSpeeds);
   static_cast<void>(dtheta);
 
diff --git a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
index 1f011f5..10996e4 100644
--- a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -12,7 +12,7 @@
 #include <networktables/NetworkTableInstance.h>
 #include <wpi/mutex.h>
 
-#include "frc/commands/Scheduler.h"
+#include "frc/smartdashboard/Sendable.h"
 #include "frc/smartdashboard/SendableBuilderImpl.h"
 #include "frc/smartdashboard/SendableRegistry.h"
 
@@ -99,19 +99,15 @@
 void LiveWindow::SetEnabled(bool enabled) {
   std::scoped_lock lock(m_impl->mutex);
   if (m_impl->liveWindowEnabled == enabled) return;
-  Scheduler* scheduler = Scheduler::GetInstance();
   m_impl->startLiveWindow = enabled;
   m_impl->liveWindowEnabled = enabled;
   // Force table generation now to make sure everything is defined
   UpdateValuesUnsafe();
   if (enabled) {
-    scheduler->SetEnabled(false);
-    scheduler->RemoveAll();
   } else {
     m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
       cbdata.builder.StopLiveWindowMode();
     });
-    scheduler->SetEnabled(true);
   }
   m_impl->enabledEntry.SetBoolean(enabled);
 }
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
index a50b77f..9717a8e 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/shuffleboard/ShuffleboardInstance.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
 #include <wpi/StringMap.h>
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
index 56a27dc..bce6196 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
@@ -42,7 +42,7 @@
     }
   };
 
-  wpi::mutex mutex;
+  wpi::recursive_mutex mutex;
 
   wpi::UidVector<std::unique_ptr<Component>, 32> components;
   wpi::DenseMap<void*, UID> componentMap;
@@ -310,7 +310,9 @@
     wpi::function_ref<void(CallbackData& data)> callback) const {
   assert(dataHandle >= 0);
   std::scoped_lock lock(m_impl->mutex);
-  for (auto&& comp : m_impl->components) {
+  wpi::SmallVector<Impl::Component*, 128> components;
+  for (auto&& comp : m_impl->components) components.emplace_back(comp.get());
+  for (auto comp : components) {
     if (comp->sendable && comp->liveWindow) {
       if (static_cast<size_t>(dataHandle) >= comp->data.size())
         comp->data.resize(dataHandle + 1);
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
index e8c9700..5e70a4c 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -7,7 +7,7 @@
 
 #include "frc/smartdashboard/SmartDashboard.h"
 
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
 #include <wpi/StringMap.h>
@@ -81,6 +81,10 @@
   Singleton::GetInstance().table->Delete(key);
 }
 
+nt::NetworkTableEntry SmartDashboard::GetEntry(wpi::StringRef key) {
+  return Singleton::GetInstance().table->GetEntry(key);
+}
+
 void SmartDashboard::PutData(wpi::StringRef key, Sendable* data) {
   if (data == nullptr) {
     wpi_setGlobalWPIErrorWithContext(NullParameter, "value");
diff --git a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
index 3578b1d..3991fec 100644
--- a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
+++ b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
@@ -27,10 +27,20 @@
   // Populate Row 2 and Row 3 with the derivatives of the equations above.
   // Then populate row 4 and 5 with the second derivatives.
   for (int i = 0; i < 4; i++) {
+    // Here, we are multiplying by (3 - i) to manually take the derivative. The
+    // power of the term in index 0 is 3, index 1 is 2 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
     m_coefficients.template block<2, 1>(2, i) =
         m_coefficients.template block<2, 1>(0, i) * (3 - i);
+  }
 
+  for (int i = 0; i < 3; i++) {
+    // Here, we are multiplying by (2 - i) to manually take the derivative. The
+    // power of the term in index 0 is 2, index 1 is 1 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
     m_coefficients.template block<2, 1>(4, i) =
-        m_coefficients.template block<2, 1>(2, i) * (3 - i);
+        m_coefficients.template block<2, 1>(2, i) * (2 - i);
   }
 }
diff --git a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
index f714c6f..bb8ad3c 100644
--- a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
+++ b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
@@ -27,10 +27,19 @@
   // Populate Row 2 and Row 3 with the derivatives of the equations above.
   // Then populate row 4 and 5 with the second derivatives.
   for (int i = 0; i < 6; i++) {
+    // Here, we are multiplying by (5 - i) to manually take the derivative. The
+    // power of the term in index 0 is 5, index 1 is 4 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
     m_coefficients.template block<2, 1>(2, i) =
         m_coefficients.template block<2, 1>(0, i) * (5 - i);
-
+  }
+  for (int i = 0; i < 5; i++) {
+    // Here, we are multiplying by (4 - i) to manually take the derivative. The
+    // power of the term in index 0 is 4, index 1 is 3 and so on. To find the
+    // coefficient of the derivative, we can use the power rule and multiply
+    // the existing coefficient by its power.
     m_coefficients.template block<2, 1>(4, i) =
-        m_coefficients.template block<2, 1>(2, i) * (5 - i);
+        m_coefficients.template block<2, 1>(2, i) * (4 - i);
   }
 }
diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
index 7c3fdc1..cbfc8c1 100644
--- a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
@@ -28,13 +28,25 @@
     waypoints.emplace_back(
         Translation2d{units::meter_t(xFinal[0]), units::meter_t(yFinal[0])});
 
+    // Populate tridiagonal system for clamped cubic
+    /* See:
+    https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
+    /undervisningsmateriale/chap7alecture.pdf
+    */
+
+    // Above-diagonal of tridiagonal matrix, zero-padded
     std::vector<double> a;
+    // Diagonal of tridiagonal matrix
     std::vector<double> b(waypoints.size() - 2, 4.0);
+    // Below-diagonal of tridiagonal matrix, zero-padded
     std::vector<double> c;
+    // rhs vectors
     std::vector<double> dx, dy;
+    // solution vectors
     std::vector<double> fx(waypoints.size() - 2, 0.0),
         fy(waypoints.size() - 2, 0.0);
 
+    // populate above-diagonal and below-diagonal vectors
     a.emplace_back(0);
     for (size_t i = 0; i < waypoints.size() - 3; ++i) {
       a.emplace_back(1);
@@ -42,6 +54,7 @@
     }
     c.emplace_back(0);
 
+    // populate rhs vectors
     dx.emplace_back(
         3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
         xInitial[1]);
@@ -63,6 +76,7 @@
                          waypoints[waypoints.size() - 3].Y().to<double>()) -
                     yFinal[1]);
 
+    // Compute solution to tridiagonal system
     ThomasAlgorithm(a, b, c, dx, &fx);
     ThomasAlgorithm(a, b, c, dy, &fy);
 
diff --git a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
index 9b4b4f5..7b2b2b6 100644
--- a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -7,8 +7,20 @@
 
 #include "frc/trajectory/Trajectory.h"
 
+#include <wpi/json.h>
+
 using namespace frc;
 
+bool Trajectory::State::operator==(const Trajectory::State& other) const {
+  return t == other.t && velocity == other.velocity &&
+         acceleration == other.acceleration && pose == other.pose &&
+         curvature == other.curvature;
+}
+
+bool Trajectory::State::operator!=(const Trajectory::State& other) const {
+  return !operator==(other);
+}
+
 Trajectory::State Trajectory::State::Interpolate(State endValue,
                                                  double i) const {
   // Find the new [t] value.
@@ -94,3 +106,21 @@
   return prevSample.Interpolate(sample,
                                 (t - prevSample.t) / (sample.t - prevSample.t));
 }
+
+void frc::to_json(wpi::json& json, const Trajectory::State& state) {
+  json = wpi::json{{"time", state.t.to<double>()},
+                   {"velocity", state.velocity.to<double>()},
+                   {"acceleration", state.acceleration.to<double>()},
+                   {"pose", state.pose},
+                   {"curvature", state.curvature.to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Trajectory::State& state) {
+  state.pose = json.at("pose").get<Pose2d>();
+  state.t = units::second_t{json.at("time").get<double>()};
+  state.velocity =
+      units::meters_per_second_t{json.at("velocity").get<double>()};
+  state.acceleration =
+      units::meters_per_second_squared_t{json.at("acceleration").get<double>()};
+  state.curvature = frc::curvature_t{json.at("curvature").get<double>()};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
new file mode 100644
index 0000000..f3cf30d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryUtil.h"
+
+#include <system_error>
+
+#include <wpi/SmallString.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+void TrajectoryUtil::ToPathweaverJson(const Trajectory& trajectory,
+                                      const wpi::Twine& path) {
+  std::error_code error_code;
+
+  wpi::SmallString<128> buf;
+  wpi::raw_fd_ostream output{path.toStringRef(buf), error_code};
+  if (error_code) {
+    throw std::runtime_error(("Cannot open file: " + path).str());
+  }
+
+  wpi::json json = trajectory.States();
+  output << json;
+  output.flush();
+}
+
+Trajectory TrajectoryUtil::FromPathweaverJson(const wpi::Twine& path) {
+  std::error_code error_code;
+
+  wpi::SmallString<128> buf;
+  wpi::raw_fd_istream input{path.toStringRef(buf), error_code};
+  if (error_code) {
+    throw std::runtime_error(("Cannot open file: " + path).str());
+  }
+
+  wpi::json json;
+  input >> json;
+
+  return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}
+
+std::string TrajectoryUtil::SerializeTrajectory(const Trajectory& trajectory) {
+  wpi::json json = trajectory.States();
+  return json.dump();
+}
+
+Trajectory TrajectoryUtil::DeserializeTrajectory(
+    const wpi::StringRef json_str) {
+  wpi::json json = wpi::json::parse(json_str);
+  return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
new file mode 100644
index 0000000..3d6b61c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
+
+#include <algorithm>
+#include <limits>
+
+#include <wpi/MathExtras.h>
+
+using namespace frc;
+
+DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
+    SimpleMotorFeedforward<units::meter> feedforward,
+    DifferentialDriveKinematics kinematics, units::volt_t maxVoltage)
+    : m_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_maxVoltage(maxVoltage) {}
+
+units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t velocity) {
+  return units::meters_per_second_t(std::numeric_limits<double>::max());
+}
+
+TrajectoryConstraint::MinMax
+DifferentialDriveVoltageConstraint::MinMaxAcceleration(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t speed) {
+  auto wheelSpeeds =
+      m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
+
+  auto maxWheelSpeed = std::max(wheelSpeeds.left, wheelSpeeds.right);
+  auto minWheelSpeed = std::min(wheelSpeeds.left, wheelSpeeds.right);
+
+  // Calculate maximum/minimum possible accelerations from motor dynamics
+  // and max/min wheel speeds
+  auto maxWheelAcceleration =
+      m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
+  auto minWheelAcceleration =
+      m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed);
+
+  // Robot chassis turning on radius = 1/|curvature|.  Outer wheel has radius
+  // increased by half of the trackwidth T.  Inner wheel has radius decreased
+  // by half of the trackwidth.  Achassis / radius = Aouter / (radius + T/2), so
+  // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 + |curvature|T/2)
+  // Inner wheel is similar.
+
+  // sgn(speed) term added to correctly account for which wheel is on
+  // outside of turn:
+  // If moving forward, max acceleration constraint corresponds to wheel on
+  // outside of turn
+  // If moving backward, max acceleration constraint corresponds to wheel on
+  // inside of turn
+  auto maxChassisAcceleration =
+      maxWheelAcceleration /
+      (1 + m_kinematics.trackWidth * units::math::abs(curvature) *
+               wpi::sgn(speed) / (2_rad));
+  auto minChassisAcceleration =
+      minWheelAcceleration /
+      (1 - m_kinematics.trackWidth * units::math::abs(curvature) *
+               wpi::sgn(speed) / (2_rad));
+
+  // Negate acceleration corresponding to wheel on inside of turn
+  // if center of turn is inside of wheelbase
+  if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) {
+    if (speed > 0_mps) {
+      minChassisAcceleration = -minChassisAcceleration;
+    } else {
+      maxChassisAcceleration = -maxChassisAcceleration;
+    }
+  }
+
+  return {minChassisAcceleration, maxChassisAcceleration};
+}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
new file mode 100644
index 0000000..2fd8151
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
@@ -0,0 +1,35 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
+
+using namespace frc;
+
+MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
+    MecanumDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t velocity) {
+  auto xVelocity = velocity * pose.Rotation().Cos();
+  auto yVelocity = velocity * pose.Rotation().Sin();
+  auto wheelSpeeds =
+      m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature});
+  wheelSpeeds.Normalize(m_maxSpeed);
+
+  auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+}
+
+TrajectoryConstraint::MinMax
+MecanumDriveKinematicsConstraint::MinMaxAcceleration(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t speed) {
+  return {};
+}
diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp
index 65c072d..5fd824d 100644
--- a/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -14,7 +14,8 @@
 #include <cstdio>
 
 #include <cameraserver/CameraServerShared.h>
-#include <hal/HAL.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/HALBase.h>
 #include <networktables/NetworkTableInstance.h>
 
 #include "WPILibVersion.h"
@@ -117,11 +118,6 @@
 std::thread::id RobotBase::GetThreadId() { return m_threadId; }
 
 RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
-  if (!HAL_Initialize(500, 0)) {
-    wpi::errs() << "FATAL ERROR: HAL could not be initialized\n";
-    wpi::errs().flush();
-    std::terminate();
-  }
   m_threadId = std::this_thread::get_id();
 
   SetupCameraServerShared();
diff --git a/wpilibc/src/main/native/include/frc/AddressableLED.h b/wpilibc/src/main/native/include/frc/AddressableLED.h
new file mode 100644
index 0000000..2ffe0ea
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/AddressableLED.h
@@ -0,0 +1,139 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/AddressableLEDTypes.h>
+#include <hal/Types.h>
+#include <units/units.h>
+#include <wpi/ArrayRef.h>
+
+#include "frc/ErrorBase.h"
+
+namespace frc {
+
+/**
+ * A class for driving addressable LEDs, such as WS2812s and NeoPixels.
+ */
+class AddressableLED : public ErrorBase {
+ public:
+  class LEDData : public HAL_AddressableLEDData {
+   public:
+    LEDData() : LEDData(0, 0, 0) {}
+    LEDData(int _r, int _g, int _b) {
+      r = _r;
+      g = _g;
+      b = _b;
+      padding = 0;
+    }
+
+    /**
+     * A helper method to set all values of the LED.
+     *
+     * @param r the r value [0-255]
+     * @param g the g value [0-255]
+     * @param b the b value [0-255]
+     */
+    void SetRGB(int r, int g, int b) {
+      this->r = r;
+      this->g = g;
+      this->b = b;
+    }
+
+    /**
+     * A helper method to set all values of the LED.
+     *
+     * @param h the h value [0-180]
+     * @param s the s value [0-255]
+     * @param v the v value [0-255]
+     */
+    void SetHSV(int h, int s, int v);
+  };
+
+  /**
+   * Constructs a new driver for a specific port.
+   *
+   * @param port the output port to use (Must be a PWM port)
+   */
+  explicit AddressableLED(int port);
+
+  ~AddressableLED() override;
+
+  /**
+   * Sets the length of the LED strip.
+   *
+   * <p>Calling this is an expensive call, so its best to call it once, then
+   * just update data.
+   *
+   * @param length the strip length
+   */
+  void SetLength(int length);
+
+  /**
+   * Sets the led output data.
+   *
+   * <p>If the output is enabled, this will start writing the next data cycle.
+   * It is safe to call, even while output is enabled.
+   *
+   * @param ledData the buffer to write
+   */
+  void SetData(wpi::ArrayRef<LEDData> ledData);
+
+  /**
+   * Sets the led output data.
+   *
+   * <p>If the output is enabled, this will start writing the next data cycle.
+   * It is safe to call, even while output is enabled.
+   *
+   * @param ledData the buffer to write
+   */
+  void SetData(std::initializer_list<LEDData> ledData);
+
+  /**
+   * Sets the bit timing.
+   *
+   * <p>By default, the driver is set up to drive WS2812s, so nothing needs to
+   * be set for those.
+   *
+   * @param lowTime0 low time for 0 bit
+   * @param highTime0 high time for 0 bit
+   * @param lowTime1 low time for 1 bit
+   * @param highTime1 high time for 1 bit
+   */
+  void SetBitTiming(units::nanosecond_t lowTime0, units::nanosecond_t highTime0,
+                    units::nanosecond_t lowTime1,
+                    units::nanosecond_t highTime1);
+
+  /**
+   * Sets the sync time.
+   *
+   * <p>The sync time is the time to hold output so LEDs enable. Default set for
+   * WS2812.
+   *
+   * @param syncTimeMicroSeconds the sync time
+   */
+  void SetSyncTime(units::microsecond_t syncTime);
+
+  /**
+   * Starts the output.
+   *
+   * <p>The output writes continously.
+   */
+  void Start();
+
+  /**
+   * Stops the output.
+   */
+  void Stop();
+
+ private:
+  hal::Handle<HAL_DigitalHandle> m_pwmHandle;
+  hal::Handle<HAL_AddressableLEDHandle> m_handle;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
new file mode 100644
index 0000000..872db87
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/SimDevice.h>
+#include <hal/Types.h>
+#include <units/units.h>
+
+#include "frc/AnalogTrigger.h"
+#include "frc/Counter.h"
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+class AnalogInput;
+
+/**
+ * Class for supporting continuous analog encoders, such as the US Digital MA3.
+ */
+class AnalogEncoder : public ErrorBase,
+                      public Sendable,
+                      public SendableHelper<AnalogEncoder> {
+ public:
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * @param analogInput the analog input to attach to
+   */
+  explicit AnalogEncoder(AnalogInput& analogInput);
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * @param analogInput the analog input to attach to
+   */
+  explicit AnalogEncoder(AnalogInput* analogInput);
+
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogInput.
+   *
+   * @param analogInput the analog input to attach to
+   */
+  explicit AnalogEncoder(std::shared_ptr<AnalogInput> analogInput);
+
+  ~AnalogEncoder() override = default;
+
+  AnalogEncoder(AnalogEncoder&&) = default;
+  AnalogEncoder& operator=(AnalogEncoder&&) = default;
+
+  /**
+   * Reset the Encoder distance to zero.
+   */
+  void Reset();
+
+  /**
+   * Get the encoder value since the last reset.
+   *
+   * This is reported in rotations since the last reset.
+   *
+   * @return the encoder value in rotations
+   */
+  units::turn_t Get() const;
+
+  /**
+   * Get the offset of position relative to the last reset.
+   *
+   * GetPositionInRotation() - GetPositionOffset() will give an encoder absolute
+   * position relative to the last reset. This could potentially be negative,
+   * which needs to be accounted for.
+   *
+   * @return the position offset
+   */
+  double GetPositionOffset() const;
+
+  /**
+   * Set the distance per rotation of the encoder. This sets the multiplier used
+   * to determine the distance driven based on the rotation value from the
+   * encoder. Set this value based on the how far the mechanism travels in 1
+   * rotation of the encoder, and factor in gearing reductions following the
+   * encoder shaft. This distance can be in any units you like, linear or
+   * angular.
+   *
+   * @param distancePerRotation the distance per rotation of the encoder
+   */
+  void SetDistancePerRotation(double distancePerRotation);
+
+  /**
+   * Get the distance per rotation for this encoder.
+   *
+   * @return The scale factor that will be used to convert rotation to useful
+   * units.
+   */
+  double GetDistancePerRotation() const;
+
+  /**
+   * Get the distance the sensor has driven since the last reset as scaled by
+   * the value from SetDistancePerRotation.
+   *
+   * @return The distance driven since the last reset
+   */
+  double GetDistance() const;
+
+  void InitSendable(SendableBuilder& builder) override;
+
+ private:
+  void Init();
+
+  std::shared_ptr<AnalogInput> m_analogInput;
+  AnalogTrigger m_analogTrigger;
+  Counter m_counter;
+  double m_positionOffset = 0;
+  double m_distancePerRotation = 1.0;
+  mutable units::turn_t m_lastPosition{0.0};
+
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simPosition;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/AnalogInput.h b/wpilibc/src/main/native/include/frc/AnalogInput.h
index 6567542..3d14c4c 100644
--- a/wpilibc/src/main/native/include/frc/AnalogInput.h
+++ b/wpilibc/src/main/native/include/frc/AnalogInput.h
@@ -19,6 +19,8 @@
 namespace frc {
 
 class SendableBuilder;
+class DMA;
+class DMASample;
 
 /**
  * Analog input class.
@@ -38,6 +40,8 @@
                     public SendableHelper<AnalogInput> {
   friend class AnalogTrigger;
   friend class AnalogGyro;
+  friend class DMA;
+  friend class DMASample;
 
  public:
   static constexpr int kAccumulatorModuleNumber = 1;
diff --git a/wpilibc/src/main/native/include/frc/AnalogTrigger.h b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
index 6a57f8a..7554bd9 100644
--- a/wpilibc/src/main/native/include/frc/AnalogTrigger.h
+++ b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
@@ -19,6 +19,7 @@
 namespace frc {
 
 class AnalogInput;
+class DutyCycle;
 class SendableBuilder;
 
 class AnalogTrigger : public ErrorBase,
@@ -45,6 +46,13 @@
    */
   explicit AnalogTrigger(AnalogInput* channel);
 
+  /**
+   * Construct an analog trigger given a duty cycle input.
+   *
+   * @param channel The pointer to the existing DutyCycle object
+   */
+  explicit AnalogTrigger(DutyCycle* dutyCycle);
+
   ~AnalogTrigger() override;
 
   AnalogTrigger(AnalogTrigger&& rhs);
@@ -61,6 +69,16 @@
   void SetLimitsVoltage(double lower, double upper);
 
   /**
+   * Set the upper and lower duty cycle limits of the analog trigger.
+   *
+   * The limits are given as floating point values between 0 and 1.
+   *
+   * @param lower The lower limit of the trigger in percentage.
+   * @param upper The upper limit of the trigger in percentage.
+   */
+  void SetLimitsDutyCycle(double lower, double upper);
+
+  /**
    * Set the upper and lower limits of the analog trigger.
    *
    * The limits are given in ADC codes.  If oversampling is used, the units must
@@ -83,6 +101,17 @@
   void SetAveraged(bool useAveragedValue);
 
   /**
+   * Configure the analog trigger to use the duty cycle vs. raw values.
+   *
+   * If the value is true, then the duty cycle value is selected for the analog
+   * trigger, otherwise the immediate value is used.
+   *
+   * @param useDutyCycle If true, use the duty cycle value, otherwise use the
+   *                         instantaneous reading
+   */
+  void SetDutyCycle(bool useDutyCycle);
+
+  /**
    * Configure the analog trigger to use a filtered value.
    *
    * The analog trigger will operate with a 3 point average rejection filter.
@@ -139,9 +168,9 @@
   void InitSendable(SendableBuilder& builder) override;
 
  private:
-  int m_index;
   hal::Handle<HAL_AnalogTriggerHandle> m_trigger;
   AnalogInput* m_analogInput = nullptr;
+  DutyCycle* m_dutyCycle = nullptr;
   bool m_ownsAnalog = false;
 };
 
diff --git a/wpilibc/src/main/native/include/frc/Counter.h b/wpilibc/src/main/native/include/frc/Counter.h
index 50e3948..2770a02 100644
--- a/wpilibc/src/main/native/include/frc/Counter.h
+++ b/wpilibc/src/main/native/include/frc/Counter.h
@@ -21,6 +21,8 @@
 
 class DigitalGlitchFilter;
 class SendableBuilder;
+class DMA;
+class DMASample;
 
 /**
  * Class for counting the number of ticks on a digital input channel.
@@ -36,6 +38,9 @@
                 public CounterBase,
                 public Sendable,
                 public SendableHelper<Counter> {
+  friend class DMA;
+  friend class DMASample;
+
  public:
   enum Mode {
     kTwoPulse = 0,
diff --git a/wpilibc/src/main/native/include/frc/DMA.h b/wpilibc/src/main/native/include/frc/DMA.h
new file mode 100644
index 0000000..57cdd27
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DMA.h
@@ -0,0 +1,57 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <hal/Types.h>
+
+#include "frc/ErrorBase.h"
+
+namespace frc {
+class Encoder;
+class Counter;
+class DigitalSource;
+class DutyCycle;
+class AnalogInput;
+class DMASample;
+
+class DMA : public ErrorBase {
+  friend class DMASample;
+
+ public:
+  DMA();
+  ~DMA() override;
+
+  DMA& operator=(DMA&& other) = default;
+  DMA(DMA&& other) = default;
+
+  void SetPause(bool pause);
+  void SetRate(int cycles);
+
+  void AddEncoder(const Encoder* encoder);
+  void AddEncoderPeriod(const Encoder* encoder);
+
+  void AddCounter(const Counter* counter);
+  void AddCounterPeriod(const Counter* counter);
+
+  void AddDigitalSource(const DigitalSource* digitalSource);
+
+  void AddDutyCycle(const DutyCycle* digitalSource);
+
+  void AddAnalogInput(const AnalogInput* analogInput);
+  void AddAveragedAnalogInput(const AnalogInput* analogInput);
+  void AddAnalogAccumulator(const AnalogInput* analogInput);
+
+  void SetExternalTrigger(DigitalSource* source, bool rising, bool falling);
+
+  void StartDMA(int queueDepth);
+  void StopDMA();
+
+ private:
+  hal::Handle<HAL_DMAHandle> dmaHandle;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DMASample.h b/wpilibc/src/main/native/include/frc/DMASample.h
new file mode 100644
index 0000000..4592930
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DMASample.h
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <type_traits>
+
+#include <hal/AnalogInput.h>
+#include <hal/DMA.h>
+#include <units/units.h>
+
+#include "frc/AnalogInput.h"
+#include "frc/Counter.h"
+#include "frc/DMA.h"
+#include "frc/DutyCycle.h"
+#include "frc/Encoder.h"
+
+namespace frc {
+class DMASample : public HAL_DMASample {
+ public:
+  HAL_DMAReadStatus Update(const DMA* dma, units::second_t timeout,
+                           int32_t* remaining, int32_t* status) {
+    units::millisecond_t ms = timeout;
+    auto timeoutMs = ms.to<int32_t>();
+    return HAL_ReadDMA(dma->dmaHandle, this, timeoutMs, remaining, status);
+  }
+
+  uint64_t GetTime() const { return timeStamp; }
+
+  units::second_t GetTimeStamp() const {
+    return units::second_t{static_cast<double>(GetTime()) * 1.0e-6};
+  }
+
+  int32_t GetEncoderRaw(const Encoder* encoder, int32_t* status) const {
+    return HAL_GetDMASampleEncoderRaw(this, encoder->m_encoder, status);
+  }
+
+  double GetEncoderDistance(const Encoder* encoder, int32_t* status) const {
+    double val = GetEncoderRaw(encoder, status);
+    val *= encoder->DecodingScaleFactor();
+    val *= encoder->GetDistancePerPulse();
+    return val;
+  }
+
+  int32_t GetEncoderPeriodRaw(const Encoder* encoder, int32_t* status) const {
+    return HAL_GetDMASampleEncoderPeriodRaw(this, encoder->m_encoder, status);
+  }
+
+  int32_t GetCounter(const Counter* counter, int32_t* status) const {
+    return HAL_GetDMASampleCounter(this, counter->m_counter, status);
+  }
+
+  int32_t GetCounterPeriod(const Counter* counter, int32_t* status) const {
+    return HAL_GetDMASampleCounterPeriod(this, counter->m_counter, status);
+  }
+
+  bool GetDigitalSource(const DigitalSource* digitalSource,
+                        int32_t* status) const {
+    return HAL_GetDMASampleDigitalSource(
+        this, digitalSource->GetPortHandleForRouting(), status);
+  }
+
+  int32_t GetAnalogInputRaw(const AnalogInput* analogInput,
+                            int32_t* status) const {
+    return HAL_GetDMASampleAnalogInputRaw(this, analogInput->m_port, status);
+  }
+
+  double GetAnalogInputVoltage(const AnalogInput* analogInput,
+                               int32_t* status) {
+    return HAL_GetAnalogValueToVolts(
+        analogInput->m_port, GetAnalogInputRaw(analogInput, status), status);
+  }
+
+  int32_t GetAveragedAnalogInputRaw(const AnalogInput* analogInput,
+                                    int32_t* status) const {
+    return HAL_GetDMASampleAveragedAnalogInputRaw(this, analogInput->m_port,
+                                                  status);
+  }
+
+  double GetAveragedAnalogInputVoltage(const AnalogInput* analogInput,
+                                       int32_t* status) {
+    return HAL_GetAnalogValueToVolts(
+        analogInput->m_port, GetAveragedAnalogInputRaw(analogInput, status),
+        status);
+  }
+
+  void GetAnalogAccumulator(const AnalogInput* analogInput, int64_t* count,
+                            int64_t* value, int32_t* status) const {
+    return HAL_GetDMASampleAnalogAccumulator(this, analogInput->m_port, count,
+                                             value, status);
+  }
+
+  int32_t GetDutyCycleOutputRaw(const DutyCycle* dutyCycle,
+                                int32_t* status) const {
+    return HAL_GetDMASampleDutyCycleOutputRaw(this, dutyCycle->m_handle,
+                                              status);
+  }
+
+  double GetDutyCycleOutput(const DutyCycle* dutyCycle, int32_t* status) {
+    return GetDutyCycleOutputRaw(dutyCycle, status) /
+           static_cast<double>(dutyCycle->GetOutputScaleFactor());
+  }
+};
+
+static_assert(std::is_standard_layout_v<frc::DMASample>,
+              "frc::DMASample must have standard layout");
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DMC60.h b/wpilibc/src/main/native/include/frc/DMC60.h
index 5a33b5a..ecf01e1 100644
--- a/wpilibc/src/main/native/include/frc/DMC60.h
+++ b/wpilibc/src/main/native/include/frc/DMC60.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,6 +13,19 @@
 
 /**
  * Digilent DMC 60 Speed Controller.
+ *
+ * Note that the DMC 60 uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the DMC 60 User
+ * Manual available from Digilent.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
  */
 class DMC60 : public PWMSpeedController {
  public:
diff --git a/wpilibc/src/main/native/include/frc/DigitalOutput.h b/wpilibc/src/main/native/include/frc/DigitalOutput.h
index 45727a4..1d1d152 100644
--- a/wpilibc/src/main/native/include/frc/DigitalOutput.h
+++ b/wpilibc/src/main/native/include/frc/DigitalOutput.h
@@ -9,7 +9,7 @@
 
 #include <hal/Types.h>
 
-#include "frc/ErrorBase.h"
+#include "frc/DigitalSource.h"
 #include "frc/smartdashboard/Sendable.h"
 #include "frc/smartdashboard/SendableHelper.h"
 
@@ -24,7 +24,7 @@
  * elsewhere will allocate channels automatically so for those devices it
  * shouldn't be done here.
  */
-class DigitalOutput : public ErrorBase,
+class DigitalOutput : public DigitalSource,
                       public Sendable,
                       public SendableHelper<DigitalOutput> {
  public:
@@ -59,10 +59,26 @@
    */
   bool Get() const;
 
+  // Digital Source Interface
+  /**
+   * @return The HAL Handle to the specified source.
+   */
+  HAL_Handle GetPortHandleForRouting() const override;
+
+  /**
+   * @return The type of analog trigger output to be used. 0 for Digitals
+   */
+  AnalogTriggerType GetAnalogTriggerTypeForRouting() const override;
+
+  /**
+   * Is source an AnalogTrigger
+   */
+  bool IsAnalogTrigger() const override;
+
   /**
    * @return The GPIO channel number that this object represents.
    */
-  int GetChannel() const;
+  int GetChannel() const override;
 
   /**
    * Output a single pulse on the digital output line.
diff --git a/wpilibc/src/main/native/include/frc/DriverStation.h b/wpilibc/src/main/native/include/frc/DriverStation.h
index 8abffde..4508701 100644
--- a/wpilibc/src/main/native/include/frc/DriverStation.h
+++ b/wpilibc/src/main/native/include/frc/DriverStation.h
@@ -394,6 +394,11 @@
    */
   void InTest(bool entering) { m_userInTest = entering; }
 
+  /**
+   * Forces WaitForData() to return immediately.
+   */
+  void WakeupWaitForData();
+
  protected:
   /**
    * Copy data from the DS task for the user.
diff --git a/wpilibc/src/main/native/include/frc/DutyCycle.h b/wpilibc/src/main/native/include/frc/DutyCycle.h
new file mode 100644
index 0000000..f0fc2d8
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DutyCycle.h
@@ -0,0 +1,135 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+class DigitalSource;
+class AnalogTrigger;
+class DMA;
+class DMASample;
+
+/**
+ * Class to read a duty cycle PWM input.
+ *
+ * <p>PWM input signals are specified with a frequency and a ratio of high to
+ * low in that frequency. There are 8 of these in the roboRIO, and they can be
+ * attached to any DigitalSource.
+ *
+ * <p>These can be combined as the input of an AnalogTrigger to a Counter in
+ * order to implement rollover checking.
+ *
+ */
+class DutyCycle : public ErrorBase,
+                  public Sendable,
+                  public SendableHelper<DutyCycle> {
+  friend class AnalogTrigger;
+  friend class DMA;
+  friend class DMASample;
+
+ public:
+  /**
+   * Constructs a DutyCycle input from a DigitalSource input.
+   *
+   * <p> This class does not own the inputted source.
+   *
+   * @param source The DigitalSource to use.
+   */
+  explicit DutyCycle(DigitalSource& source);
+  /**
+   * Constructs a DutyCycle input from a DigitalSource input.
+   *
+   * <p> This class does not own the inputted source.
+   *
+   * @param source The DigitalSource to use.
+   */
+  explicit DutyCycle(DigitalSource* source);
+  /**
+   * Constructs a DutyCycle input from a DigitalSource input.
+   *
+   * <p> This class does not own the inputted source.
+   *
+   * @param source The DigitalSource to use.
+   */
+  explicit DutyCycle(std::shared_ptr<DigitalSource> source);
+
+  /**
+   * Close the DutyCycle and free all resources.
+   */
+  ~DutyCycle() override;
+
+  DutyCycle(DutyCycle&&) = default;
+  DutyCycle& operator=(DutyCycle&&) = default;
+
+  /**
+   * Get the frequency of the duty cycle signal.
+   *
+   * @return frequency in Hertz
+   */
+  int GetFrequency() const;
+
+  /**
+   * Get the output ratio of the duty cycle signal.
+   *
+   * <p> 0 means always low, 1 means always high.
+   *
+   * @return output ratio between 0 and 1
+   */
+  double GetOutput() const;
+
+  /**
+   * Get the raw output ratio of the duty cycle signal.
+   *
+   * <p> 0 means always low, an output equal to
+   * GetOutputScaleFactor() means always high.
+   *
+   * @return output ratio in raw units
+   */
+  unsigned int GetOutputRaw() const;
+
+  /**
+   * Get the scale factor of the output.
+   *
+   * <p> An output equal to this value is always high, and then linearly scales
+   * down to 0. Divide the result of getOutputRaw by this in order to get the
+   * percentage between 0 and 1.
+   *
+   * @return the output scale factor
+   */
+  unsigned int GetOutputScaleFactor() const;
+
+  /**
+   * Get the FPGA index for the DutyCycle.
+   *
+   * @return the FPGA index
+   */
+  int GetFPGAIndex() const;
+
+  /**
+   * Get the channel of the source.
+   *
+   * @return the source channel
+   */
+  int GetSourceChannel() const;
+
+ protected:
+  void InitSendable(SendableBuilder& builder) override;
+
+ private:
+  void InitDutyCycle();
+  std::shared_ptr<DigitalSource> m_source;
+  hal::Handle<HAL_DutyCycleHandle> m_handle;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
new file mode 100644
index 0000000..92864a8
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
@@ -0,0 +1,174 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+
+#include <hal/SimDevice.h>
+#include <hal/Types.h>
+#include <units/units.h>
+
+#include "frc/AnalogTrigger.h"
+#include "frc/Counter.h"
+#include "frc/ErrorBase.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+
+namespace frc {
+class DutyCycle;
+class DigitalSource;
+
+/**
+ * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with
+ * PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
+ * Encoder.
+ */
+class DutyCycleEncoder : public ErrorBase,
+                         public Sendable,
+                         public SendableHelper<DutyCycleEncoder> {
+ public:
+  /**
+   * Construct a new DutyCycleEncoder on a specific channel.
+   *
+   * @param channel the channel to attach to
+   */
+  explicit DutyCycleEncoder(int channel);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   */
+  explicit DutyCycleEncoder(DutyCycle& dutyCycle);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   */
+  explicit DutyCycleEncoder(DutyCycle* dutyCycle);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to an existing DutyCycle object.
+   *
+   * @param dutyCycle the duty cycle to attach to
+   */
+  explicit DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to a DigitalSource object.
+   *
+   * @param source the digital source to attach to
+   */
+  explicit DutyCycleEncoder(DigitalSource& digitalSource);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to a DigitalSource object.
+   *
+   * @param source the digital source to attach to
+   */
+  explicit DutyCycleEncoder(DigitalSource* digitalSource);
+
+  /**
+   * Construct a new DutyCycleEncoder attached to a DigitalSource object.
+   *
+   * @param source the digital source to attach to
+   */
+  explicit DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource);
+
+  ~DutyCycleEncoder() override = default;
+
+  DutyCycleEncoder(DutyCycleEncoder&&) = default;
+  DutyCycleEncoder& operator=(DutyCycleEncoder&&) = default;
+
+  /**
+   * Get the frequency in Hz of the duty cycle signal from the encoder.
+   *
+   * @return duty cycle frequency in Hz
+   */
+  int GetFrequency() const;
+
+  /**
+   * Get if the sensor is connected
+   *
+   * This uses the duty cycle frequency to determine if the sensor is connected.
+   * By default, a value of 100 Hz is used as the threshold, and this value can
+   * be changed with SetConnectedFrequencyThreshold.
+   *
+   * @return true if the sensor is connected
+   */
+  bool IsConnected() const;
+
+  /**
+   * Change the frequency threshold for detecting connection used by
+   * IsConnected.
+   *
+   * @param frequency the minimum frequency in Hz.
+   */
+  void SetConnectedFrequencyThreshold(int frequency);
+
+  /**
+   * Reset the Encoder distance to zero.
+   */
+  void Reset();
+
+  /**
+   * Get the encoder value since the last reset.
+   *
+   * This is reported in rotations since the last reset.
+   *
+   * @return the encoder value in rotations
+   */
+  units::turn_t Get() const;
+
+  /**
+   * Set the distance per rotation of the encoder. This sets the multiplier used
+   * to determine the distance driven based on the rotation value from the
+   * encoder. Set this value based on the how far the mechanism travels in 1
+   * rotation of the encoder, and factor in gearing reductions following the
+   * encoder shaft. This distance can be in any units you like, linear or
+   * angular.
+   *
+   * @param distancePerRotation the distance per rotation of the encoder
+   */
+  void SetDistancePerRotation(double distancePerRotation);
+
+  /**
+   * Get the distance per rotation for this encoder.
+   *
+   * @return The scale factor that will be used to convert rotation to useful
+   * units.
+   */
+  double GetDistancePerRotation() const;
+
+  /**
+   * Get the distance the sensor has driven since the last reset as scaled by
+   * the value from SetDistancePerRotation.
+   *
+   * @return The distance driven since the last reset
+   */
+  double GetDistance() const;
+
+  void InitSendable(SendableBuilder& builder) override;
+
+ private:
+  void Init();
+
+  std::shared_ptr<DutyCycle> m_dutyCycle;
+  AnalogTrigger m_analogTrigger;
+  Counter m_counter;
+  int m_frequencyThreshold = 100;
+  double m_positionOffset = 0;
+  double m_distancePerRotation = 1.0;
+  mutable units::turn_t m_lastPosition{0.0};
+
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simPosition;
+  hal::SimBoolean m_simIsConnected;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Encoder.h b/wpilibc/src/main/native/include/frc/Encoder.h
index 074cc5e..60552f6 100644
--- a/wpilibc/src/main/native/include/frc/Encoder.h
+++ b/wpilibc/src/main/native/include/frc/Encoder.h
@@ -23,6 +23,8 @@
 class DigitalSource;
 class DigitalGlitchFilter;
 class SendableBuilder;
+class DMA;
+class DMASample;
 
 /**
  * Class to read quad encoders.
@@ -44,6 +46,9 @@
                 public PIDSource,
                 public Sendable,
                 public SendableHelper<Encoder> {
+  friend class DMA;
+  friend class DMASample;
+
  public:
   enum IndexingType {
     kResetWhileHigh,
diff --git a/wpilibc/src/main/native/include/frc/ErrorBase.h b/wpilibc/src/main/native/include/frc/ErrorBase.h
index 7235352..e9168aa 100644
--- a/wpilibc/src/main/native/include/frc/ErrorBase.h
+++ b/wpilibc/src/main/native/include/frc/ErrorBase.h
@@ -16,6 +16,9 @@
 #include "frc/Base.h"
 #include "frc/Error.h"
 
+// Forward declared manually to avoid needing to pull in entire HAL header.
+extern "C" const char* HAL_GetErrorMessage(int32_t code);
+
 #define wpi_setErrnoErrorWithContext(context) \
   this->SetErrnoError((context), __FILE__, __FUNCTION__, __LINE__)
 #define wpi_setErrnoError() wpi_setErrnoErrorWithContext("")
@@ -35,6 +38,22 @@
       this->SetErrorRange((code), (min), (max), (req), (context), __FILE__, \
                           __FUNCTION__, __LINE__);                          \
   } while (0)
+
+#define wpi_setHALError(code)                                     \
+  do {                                                            \
+    if ((code) != 0)                                              \
+      this->SetError((code), HAL_GetErrorMessage(code), __FILE__, \
+                     __FUNCTION__, __LINE__);                     \
+  } while (0)
+
+#define wpi_setHALErrorWithRange(code, min, max, req)                        \
+  do {                                                                       \
+    if ((code) != 0)                                                         \
+      this->SetErrorRange((code), (min), (max), (req),                       \
+                          HAL_GetErrorMessage(code), __FILE__, __FUNCTION__, \
+                          __LINE__);                                         \
+  } while (0)
+
 #define wpi_setError(code) wpi_setErrorWithContext(code, "")
 #define wpi_setStaticErrorWithContext(object, code, context)                 \
   do {                                                                       \
@@ -43,12 +62,21 @@
   } while (0)
 #define wpi_setStaticError(object, code) \
   wpi_setStaticErrorWithContext(object, code, "")
+
 #define wpi_setGlobalErrorWithContext(code, context)                \
   do {                                                              \
     if ((code) != 0)                                                \
       ::frc::ErrorBase::SetGlobalError((code), (context), __FILE__, \
                                        __FUNCTION__, __LINE__);     \
   } while (0)
+
+#define wpi_setGlobalHALError(code)                                       \
+  do {                                                                    \
+    if ((code) != 0)                                                      \
+      ::frc::ErrorBase::SetGlobalError((code), HAL_GetErrorMessage(code), \
+                                       __FILE__, __FUNCTION__, __LINE__); \
+  } while (0)
+
 #define wpi_setGlobalError(code) wpi_setGlobalErrorWithContext(code, "")
 #define wpi_setWPIErrorWithContext(error, context)                    \
   this->SetWPIError(wpi_error_s_##error(), wpi_error_value_##error(), \
diff --git a/wpilibc/src/main/native/include/frc/IterativeRobot.h b/wpilibc/src/main/native/include/frc/IterativeRobot.h
index 8ce6356..447c477 100644
--- a/wpilibc/src/main/native/include/frc/IterativeRobot.h
+++ b/wpilibc/src/main/native/include/frc/IterativeRobot.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <atomic>
+
 #include "frc/IterativeRobotBase.h"
 
 namespace frc {
@@ -28,9 +30,6 @@
   IterativeRobot();
   virtual ~IterativeRobot() = default;
 
-  IterativeRobot(IterativeRobot&&) = default;
-  IterativeRobot& operator=(IterativeRobot&&) = default;
-
   /**
    * Provide an alternate "main loop" via StartCompetition().
    *
@@ -38,6 +37,14 @@
    * with the DS packets.
    */
   void StartCompetition() override;
+
+  /**
+   * Ends the main loop in StartCompetition().
+   */
+  void EndCompetition() override;
+
+ private:
+  std::atomic<bool> m_exit{false};
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Jaguar.h b/wpilibc/src/main/native/include/frc/Jaguar.h
index f22eb42..7a8503e 100644
--- a/wpilibc/src/main/native/include/frc/Jaguar.h
+++ b/wpilibc/src/main/native/include/frc/Jaguar.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,6 +13,19 @@
 
 /**
  * Luminary Micro / Vex Robotics Jaguar Speed Controller with PWM control.
+ *
+ * Note that the Jaguar uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended. The
+ * calibration procedure can be found in the Jaguar User Manual available from
+ * Vex.
+ *
+ * \li 2.310ms = full "forward"
+ * \li 1.550ms = the "high end" of the deadband range
+ * \li 1.507ms = center of the deadband range (off)
+ * \li 1.454ms = the "low end" of the deadband range
+ * \li 0.697ms = full "reverse"
  */
 class Jaguar : public PWMSpeedController {
  public:
diff --git a/wpilibc/src/main/native/include/frc/LinearFilter.h b/wpilibc/src/main/native/include/frc/LinearFilter.h
index de52003..2b83a32 100644
--- a/wpilibc/src/main/native/include/frc/LinearFilter.h
+++ b/wpilibc/src/main/native/include/frc/LinearFilter.h
@@ -7,9 +7,12 @@
 
 #pragma once
 
+#include <cassert>
+#include <cmath>
 #include <initializer_list>
 #include <vector>
 
+#include <hal/FRCUsageReporting.h>
 #include <units/units.h>
 #include <wpi/ArrayRef.h>
 #include <wpi/circular_buffer.h>
@@ -66,6 +69,7 @@
  * Combining this with Note 1 - the impetus is on YOU as a developer to make
  * sure Calculate() gets called at the desired, constant frequency!
  */
+template <class T>
 class LinearFilter {
  public:
   /**
@@ -74,7 +78,15 @@
    * @param ffGains The "feed forward" or FIR gains.
    * @param fbGains The "feed back" or IIR gains.
    */
-  LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains);
+  LinearFilter(wpi::ArrayRef<double> ffGains, wpi::ArrayRef<double> fbGains)
+      : m_inputs(ffGains.size()),
+        m_outputs(fbGains.size()),
+        m_inputGains(ffGains),
+        m_outputGains(fbGains) {
+    static int instances = 0;
+    instances++;
+    HAL_Report(HALUsageReporting::kResourceType_LinearFilter, instances);
+  }
 
   /**
    * Create a linear FIR or IIR filter.
@@ -99,8 +111,11 @@
    * @param period       The period in seconds between samples taken by the
    *                     user.
    */
-  static LinearFilter SinglePoleIIR(double timeConstant,
-                                    units::second_t period);
+  static LinearFilter<T> SinglePoleIIR(double timeConstant,
+                                       units::second_t period) {
+    double gain = std::exp(-period.to<double>() / timeConstant);
+    return LinearFilter(1.0 - gain, -gain);
+  }
 
   /**
    * Creates a first-order high-pass filter of the form:<br>
@@ -113,7 +128,10 @@
    * @param period       The period in seconds between samples taken by the
    *                     user.
    */
-  static LinearFilter HighPass(double timeConstant, units::second_t period);
+  static LinearFilter<T> HighPass(double timeConstant, units::second_t period) {
+    double gain = std::exp(-period.to<double>() / timeConstant);
+    return LinearFilter({gain, -gain}, {-gain});
+  }
 
   /**
    * Creates a K-tap FIR moving average filter of the form:<br>
@@ -124,12 +142,20 @@
    * @param taps The number of samples to average over. Higher = smoother but
    *             slower
    */
-  static LinearFilter MovingAverage(int taps);
+  static LinearFilter<T> MovingAverage(int taps) {
+    assert(taps > 0);
+
+    std::vector<double> gains(taps, 1.0 / taps);
+    return LinearFilter(gains, {});
+  }
 
   /**
    * Reset the filter state.
    */
-  void Reset();
+  void Reset() {
+    m_inputs.reset();
+    m_outputs.reset();
+  }
 
   /**
    * Calculates the next value of the filter.
@@ -138,11 +164,29 @@
    *
    * @return The filtered value at this step
    */
-  double Calculate(double input);
+  T Calculate(T input) {
+    T retVal = T(0.0);
+
+    // Rotate the inputs
+    m_inputs.push_front(input);
+
+    // Calculate the new value
+    for (size_t i = 0; i < m_inputGains.size(); i++) {
+      retVal += m_inputs[i] * m_inputGains[i];
+    }
+    for (size_t i = 0; i < m_outputGains.size(); i++) {
+      retVal -= m_outputs[i] * m_outputGains[i];
+    }
+
+    // Rotate the outputs
+    m_outputs.push_front(retVal);
+
+    return retVal;
+  }
 
  private:
-  wpi::circular_buffer<double> m_inputs{0};
-  wpi::circular_buffer<double> m_outputs{0};
+  wpi::circular_buffer<T> m_inputs;
+  wpi::circular_buffer<T> m_outputs;
   std::vector<double> m_inputGains;
   std::vector<double> m_outputGains;
 };
diff --git a/wpilibc/src/main/native/include/frc/MedianFilter.h b/wpilibc/src/main/native/include/frc/MedianFilter.h
new file mode 100644
index 0000000..b5d499b
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/MedianFilter.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <algorithm>
+#include <vector>
+
+#include <wpi/Algorithm.h>
+#include <wpi/circular_buffer.h>
+
+namespace frc {
+/**
+ * A class that implements a moving-window median filter.  Useful for reducing
+ * measurement noise, especially with processes that generate occasional,
+ * extreme outliers (such as values from vision processing, LIDAR, or ultrasonic
+ * sensors).
+ */
+template <class T>
+class MedianFilter {
+ public:
+  /**
+   * Creates a new MedianFilter.
+   *
+   * @param size The number of samples in the moving window.
+   */
+  explicit MedianFilter(size_t size) : m_valueBuffer(size), m_size{size} {}
+
+  /**
+   * Calculates the moving-window median for the next value of the input stream.
+   *
+   * @param next The next input value.
+   * @return The median of the moving window, updated to include the next value.
+   */
+  T Calculate(T next) {
+    // Insert next value at proper point in sorted array
+    wpi::insert_sorted(m_orderedValues, next);
+
+    size_t curSize = m_orderedValues.size();
+
+    // If buffer is at max size, pop element off of end of circular buffer
+    // and remove from ordered list
+    if (curSize > m_size) {
+      m_orderedValues.erase(std::find(m_orderedValues.begin(),
+                                      m_orderedValues.end(),
+                                      m_valueBuffer.pop_back()));
+      curSize = curSize - 1;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.push_front(next);
+
+    if (curSize % 2 == 1) {
+      // If size is odd, return middle element of sorted list
+      return m_orderedValues[curSize / 2];
+    } else {
+      // If size is even, return average of middle elements
+      return (m_orderedValues[curSize / 2 - 1] + m_orderedValues[curSize / 2]) /
+             2.0;
+    }
+  }
+
+  /**
+   * Resets the filter, clearing the window of all elements.
+   */
+  void Reset() {
+    m_orderedValues.clear();
+    m_valueBuffer.reset();
+  }
+
+ private:
+  wpi::circular_buffer<T> m_valueBuffer;
+  std::vector<T> m_orderedValues;
+  size_t m_size;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Notifier.h b/wpilibc/src/main/native/include/frc/Notifier.h
index f2f37f1..24ffba3 100644
--- a/wpilibc/src/main/native/include/frc/Notifier.h
+++ b/wpilibc/src/main/native/include/frc/Notifier.h
@@ -16,6 +16,7 @@
 
 #include <hal/Types.h>
 #include <units/units.h>
+#include <wpi/Twine.h>
 #include <wpi/deprecated.h>
 #include <wpi/mutex.h>
 
@@ -47,6 +48,13 @@
   Notifier& operator=(Notifier&& rhs);
 
   /**
+   * Sets the name of the notifier.  Used for debugging purposes only.
+   *
+   * @param name Name
+   */
+  void SetName(const wpi::Twine& name);
+
+  /**
    * Change the handler function.
    *
    * @param handler Handler
diff --git a/wpilibc/src/main/native/include/frc/PIDBase.h b/wpilibc/src/main/native/include/frc/PIDBase.h
index 098718f..513d46b 100644
--- a/wpilibc/src/main/native/include/frc/PIDBase.h
+++ b/wpilibc/src/main/native/include/frc/PIDBase.h
@@ -402,7 +402,7 @@
   double m_error = 0;
   double m_result = 0;
 
-  LinearFilter m_filter{{}, {}};
+  LinearFilter<double> m_filter{{}, {}};
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/PWM.h b/wpilibc/src/main/native/include/frc/PWM.h
index 58e18d2..406a93e 100644
--- a/wpilibc/src/main/native/include/frc/PWM.h
+++ b/wpilibc/src/main/native/include/frc/PWM.h
@@ -17,7 +17,7 @@
 #include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
-
+class AddressableLED;
 class SendableBuilder;
 
 /**
@@ -34,11 +34,12 @@
  *   - 1999 to 1001 = linear scaling from "full forward" to "center"
  *   - 1000 = center value
  *   - 999 to 2 = linear scaling from "center" to "full reverse"
- *   - 1 = minimum pulse width (currently .5ms)
+ *   - 1 = minimum pulse width (currently 0.5ms)
  *   - 0 = disabled (i.e. PWM output is held low)
  */
 class PWM : public MotorSafety, public Sendable, public SendableHelper<PWM> {
  public:
+  friend class AddressableLED;
   /**
    * Represents the amount to multiply the minimum servo-pulse pwm period by.
    */
diff --git a/wpilibc/src/main/native/include/frc/PWMSparkMax.h b/wpilibc/src/main/native/include/frc/PWMSparkMax.h
index c8b22d7..3ce6466 100644
--- a/wpilibc/src/main/native/include/frc/PWMSparkMax.h
+++ b/wpilibc/src/main/native/include/frc/PWMSparkMax.h
@@ -12,14 +12,27 @@
 namespace frc {
 
 /**
- * REV Robotics SparkMax Speed Controller.
+ * REV Robotics SPARK MAX Speed Controller.
+ *
+ * Note that the SPARK MAX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the SPARK MAX User
+ * Manual available from REV Robotics.
+ *
+ * \li 2.003ms = full "forward"
+ * \li 1.550ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.460ms = the "low end" of the deadband range
+ * \li 0.999ms = full "reverse"
  */
 class PWMSparkMax : public PWMSpeedController {
  public:
   /**
-   * Constructor for a SparkMax.
+   * Constructor for a SPARK MAX.
    *
-   * @param channel The PWM channel that the Spark is attached to. 0-9 are
+   * @param channel The PWM channel that the SPARK MAX is attached to. 0-9 are
    *                on-board, 10-19 are on the MXP port
    */
   explicit PWMSparkMax(int channel);
diff --git a/wpilibc/src/main/native/include/frc/PWMTalonSRX.h b/wpilibc/src/main/native/include/frc/PWMTalonSRX.h
index 9260e1e..b9c8369 100644
--- a/wpilibc/src/main/native/include/frc/PWMTalonSRX.h
+++ b/wpilibc/src/main/native/include/frc/PWMTalonSRX.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,13 +14,26 @@
 /**
  * Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM
  * control.
+ *
+ * Note that the Talon SRX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Talon SRX User
+ * Manual available from Cross The Road Electronics.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
  */
 class PWMTalonSRX : public PWMSpeedController {
  public:
   /**
-   * Construct a PWMTalonSRX connected via PWM.
+   * Construct a Talon SRX connected via PWM.
    *
-   * @param channel The PWM channel that the PWMTalonSRX is attached to. 0-9 are
+   * @param channel The PWM channel that the Talon SRX is attached to. 0-9 are
    *                on-board, 10-19 are on the MXP port
    */
   explicit PWMTalonSRX(int channel);
diff --git a/wpilibc/src/main/native/include/frc/PWMVictorSPX.h b/wpilibc/src/main/native/include/frc/PWMVictorSPX.h
index d7112c9..a19e704 100644
--- a/wpilibc/src/main/native/include/frc/PWMVictorSPX.h
+++ b/wpilibc/src/main/native/include/frc/PWMVictorSPX.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -14,13 +14,26 @@
 /**
  * Cross the Road Electronics (CTRE) Victor SPX Speed Controller with PWM
  * control.
+ *
+ * Note that the Victor SPX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor SPX User
+ * Manual available from Cross The Road Electronics.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
  */
 class PWMVictorSPX : public PWMSpeedController {
  public:
   /**
-   * Construct a PWMVictorSPX connected via PWM.
+   * Construct a Victor SPX connected via PWM.
    *
-   * @param channel The PWM channel that the PWMVictorSPX is attached to. 0-9
+   * @param channel The PWM channel that the Victor SPX is attached to. 0-9
    *                are on-board, 10-19 are on the MXP port
    */
   explicit PWMVictorSPX(int channel);
diff --git a/wpilibc/src/main/native/include/frc/RobotBase.h b/wpilibc/src/main/native/include/frc/RobotBase.h
index 85a9d12..725aa97 100644
--- a/wpilibc/src/main/native/include/frc/RobotBase.h
+++ b/wpilibc/src/main/native/include/frc/RobotBase.h
@@ -7,9 +7,12 @@
 
 #pragma once
 
+#include <chrono>
 #include <thread>
 
 #include <hal/Main.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
 #include <wpi/raw_ostream.h>
 
 #include "frc/Base.h"
@@ -23,9 +26,13 @@
 namespace impl {
 
 template <class Robot>
-void RunRobot() {
-  static Robot robot;
-  robot.StartCompetition();
+void RunRobot(wpi::mutex& m, Robot** robot) {
+  static Robot theRobot;
+  {
+    std::scoped_lock lock{m};
+    *robot = &theRobot;
+  }
+  theRobot.StartCompetition();
 }
 
 }  // namespace impl
@@ -36,20 +43,50 @@
   if (halInit != 0) {
     return halInit;
   }
+
+  static wpi::mutex m;
+  static wpi::condition_variable cv;
+  static Robot* robot = nullptr;
+  static bool exited = false;
+
   if (HAL_HasMain()) {
-    std::thread([] {
+    std::thread thr([] {
       try {
-        impl::RunRobot<Robot>();
+        impl::RunRobot<Robot>(m, &robot);
       } catch (...) {
         HAL_ExitMain();
+        {
+          std::scoped_lock lock{m};
+          robot = nullptr;
+          exited = true;
+        }
+        cv.notify_all();
         throw;
       }
+
       HAL_ExitMain();
-    })
-        .detach();
+      {
+        std::scoped_lock lock{m};
+        robot = nullptr;
+        exited = true;
+      }
+      cv.notify_all();
+    });
+
     HAL_RunMain();
+
+    // signal loop to exit
+    if (robot) robot->EndCompetition();
+
+    // prefer to join, but detach to exit if it doesn't exit in a timely manner
+    using namespace std::chrono_literals;
+    std::unique_lock lock{m};
+    if (cv.wait_for(lock, 1s, [] { return exited; }))
+      thr.join();
+    else
+      thr.detach();
   } else {
-    impl::RunRobot<Robot>();
+    impl::RunRobot<Robot>(m, &robot);
   }
 
   return 0;
@@ -127,6 +164,8 @@
 
   virtual void StartCompetition() = 0;
 
+  virtual void EndCompetition() = 0;
+
   static constexpr bool IsReal() {
 #ifdef __FRC_ROBORIO__
     return true;
diff --git a/wpilibc/src/main/native/include/frc/SD540.h b/wpilibc/src/main/native/include/frc/SD540.h
index 91666b4..07f7f18 100644
--- a/wpilibc/src/main/native/include/frc/SD540.h
+++ b/wpilibc/src/main/native/include/frc/SD540.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,6 +13,19 @@
 
 /**
  * Mindsensors SD540 Speed Controller.
+ *
+ * Note that the SD540 uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the SD540 User Manual available
+ * from Mindsensors.
+ *
+ * \li 2.05ms = full "forward"
+ * \li 1.55ms = the "high end" of the deadband range
+ * \li 1.50ms = center of the deadband range (off)
+ * \li 1.44ms = the "low end" of the deadband range
+ * \li 0.94ms = full "reverse"
  */
 class SD540 : public PWMSpeedController {
  public:
diff --git a/wpilibc/src/main/native/include/frc/Servo.h b/wpilibc/src/main/native/include/frc/Servo.h
index ae5c599..16b4281 100644
--- a/wpilibc/src/main/native/include/frc/Servo.h
+++ b/wpilibc/src/main/native/include/frc/Servo.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -105,7 +105,7 @@
   static constexpr double kMinServoAngle = 0.0;
 
   static constexpr double kDefaultMaxServoPWM = 2.4;
-  static constexpr double kDefaultMinServoPWM = .6;
+  static constexpr double kDefaultMinServoPWM = 0.6;
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Spark.h b/wpilibc/src/main/native/include/frc/Spark.h
index 640696f..24ed8f5 100644
--- a/wpilibc/src/main/native/include/frc/Spark.h
+++ b/wpilibc/src/main/native/include/frc/Spark.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -12,14 +12,27 @@
 namespace frc {
 
 /**
- * REV Robotics Speed Controller.
+ * REV Robotics SPARK Speed Controller.
+ *
+ * Note that the SPARK uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the SPARK User Manual available
+ * from REV Robotics.
+ *
+ * \li 2.003ms = full "forward"
+ * \li 1.550ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.460ms = the "low end" of the deadband range
+ * \li 0.999ms = full "reverse"
  */
 class Spark : public PWMSpeedController {
  public:
   /**
-   * Constructor for a Spark.
+   * Constructor for a SPARK.
    *
-   * @param channel The PWM channel that the Spark is attached to. 0-9 are
+   * @param channel The PWM channel that the SPARK is attached to. 0-9 are
    *                on-board, 10-19 are on the MXP port
    */
   explicit Spark(int channel);
diff --git a/wpilibc/src/main/native/include/frc/SpeedController.h b/wpilibc/src/main/native/include/frc/SpeedController.h
index 49a828b..2b11aee 100644
--- a/wpilibc/src/main/native/include/frc/SpeedController.h
+++ b/wpilibc/src/main/native/include/frc/SpeedController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <units/units.h>
+
 #include "frc/PIDOutput.h"
 
 namespace frc {
@@ -26,6 +28,20 @@
   virtual void Set(double speed) = 0;
 
   /**
+   * Sets the voltage output of the SpeedController.  Compensates for
+   * the current bus voltage to ensure that the desired voltage is output even
+   * if the battery voltage is below 12V - highly useful when the voltage
+   * outputs are "meaningful" (e.g. they come from a feedforward calculation).
+   *
+   * <p>NOTE: This function *must* be called regularly in order for voltage
+   * compensation to work properly - unlike the ordinary set function, it is not
+   * "set it and forget it."
+   *
+   * @param output The voltage to output.
+   */
+  virtual void SetVoltage(units::volt_t output);
+
+  /**
    * Common interface for getting the current set speed of a speed controller.
    *
    * @return The current set speed.  Value is between -1.0 and 1.0.
diff --git a/wpilibc/src/main/native/include/frc/Talon.h b/wpilibc/src/main/native/include/frc/Talon.h
index 9a5400f..6e92dfc 100644
--- a/wpilibc/src/main/native/include/frc/Talon.h
+++ b/wpilibc/src/main/native/include/frc/Talon.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,6 +13,19 @@
 
 /**
  * Cross the Road Electronics (CTRE) Talon and Talon SR Speed Controller.
+ *
+ * Note that the Talon uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the Talon User Manual available
+ * from CTRE.
+ *
+ * \li 2.037ms = full "forward"
+ * \li 1.539ms = the "high end" of the deadband range
+ * \li 1.513ms = center of the deadband range (off)
+ * \li 1.487ms = the "low end" of the deadband range
+ * \li 0.989ms = full "reverse"
  */
 class Talon : public PWMSpeedController {
  public:
diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h
index 1c8ff80..112e2c9 100644
--- a/wpilibc/src/main/native/include/frc/TimedRobot.h
+++ b/wpilibc/src/main/native/include/frc/TimedRobot.h
@@ -35,6 +35,11 @@
   void StartCompetition() override;
 
   /**
+   * Ends the main loop in StartCompetition().
+   */
+  void EndCompetition() override;
+
+  /**
    * Get the time period between calls to Periodic() functions.
    */
   units::second_t GetPeriod() const;
diff --git a/wpilibc/src/main/native/include/frc/Victor.h b/wpilibc/src/main/native/include/frc/Victor.h
index 5b6cba4..89c4a89 100644
--- a/wpilibc/src/main/native/include/frc/Victor.h
+++ b/wpilibc/src/main/native/include/frc/Victor.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -16,6 +16,20 @@
  *
  * The Vex Robotics Victor 884 Speed Controller can also be used with this
  * class but may need to be calibrated per the Victor 884 user manual.
+ *
+ * Note that the Victor uses the following bounds for PWM values.  These
+ * values were determined empirically and optimized for the Victor 888. These
+ * values should work reasonably well for Victor 884 controllers as well but
+ * if users experience issues such as asymmetric behavior around the deadband
+ * or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor 884 User
+ * Manual available from Vex.
+ *
+ * \li 2.027ms = full "forward"
+ * \li 1.525ms = the "high end" of the deadband range
+ * \li 1.507ms = center of the deadband range (off)
+ * \li 1.490ms = the "low end" of the deadband range
+ * \li 1.026ms = full "reverse"
  */
 class Victor : public PWMSpeedController {
  public:
diff --git a/wpilibc/src/main/native/include/frc/VictorSP.h b/wpilibc/src/main/native/include/frc/VictorSP.h
index b23fba1..d8aa8e8 100644
--- a/wpilibc/src/main/native/include/frc/VictorSP.h
+++ b/wpilibc/src/main/native/include/frc/VictorSP.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -13,11 +13,24 @@
 
 /**
  * Vex Robotics Victor SP Speed Controller.
+ *
+ * Note that the Victor SP uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor SP User
+ * Manual available from Vex.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
  */
 class VictorSP : public PWMSpeedController {
  public:
   /**
-   * Constructor for a VictorSP.
+   * Constructor for a Victor SP.
    *
    * @param channel The PWM channel that the VictorSP is attached to. 0-9 are
    *                on-board, 10-19 are on the MXP port
diff --git a/wpilibc/src/main/native/include/frc/WPILib.h b/wpilibc/src/main/native/include/frc/WPILib.h
index 69fa061..9d62514 100644
--- a/wpilibc/src/main/native/include/frc/WPILib.h
+++ b/wpilibc/src/main/native/include/frc/WPILib.h
@@ -16,8 +16,10 @@
 
 // clang-format on
 
+#if __has_include(<cameraserver/CameraServer.h>)
 #include <cameraserver/CameraServer.h>
 #include <vision/VisionRunner.h>
+#endif
 
 #include "frc/ADXL345_I2C.h"
 #include "frc/ADXL345_SPI.h"
@@ -82,6 +84,7 @@
 #include "frc/VictorSP.h"
 #include "frc/WPIErrors.h"
 #include "frc/XboxController.h"
+#if __has_include("frc/buttons/InternalButton.h")
 #include "frc/buttons/InternalButton.h"
 #include "frc/buttons/JoystickButton.h"
 #include "frc/buttons/NetworkButton.h"
@@ -90,12 +93,12 @@
 #include "frc/commands/PIDCommand.h"
 #include "frc/commands/PIDSubsystem.h"
 #include "frc/commands/PrintCommand.h"
-#include "frc/commands/Scheduler.h"
 #include "frc/commands/StartCommand.h"
 #include "frc/commands/Subsystem.h"
 #include "frc/commands/WaitCommand.h"
 #include "frc/commands/WaitForChildren.h"
 #include "frc/commands/WaitUntilCommand.h"
+#endif
 #include "frc/drive/DifferentialDrive.h"
 #include "frc/drive/KilloughDrive.h"
 #include "frc/drive/MecanumDrive.h"
diff --git a/wpilibc/src/main/native/include/frc/buttons/Button.h b/wpilibc/src/main/native/include/frc/buttons/Button.h
deleted file mode 100644
index 71641e9..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/Button.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/Trigger.h"
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-/**
- * This class provides an easy way to link commands to OI inputs.
- *
- * It is very easy to link a button to a command.  For instance, you could link
- * the trigger button of a joystick to a "score" command.
- *
- * This class represents a subclass of Trigger that is specifically aimed at
- * buttons on an operator interface as a common use case of the more generalized
- * Trigger objects. This is a simple wrapper around Trigger with the method
- * names renamed to fit the Button object use.
- */
-class Button : public Trigger {
- public:
-  Button() = default;
-  Button(Button&&) = default;
-  Button& operator=(Button&&) = default;
-
-  /**
-   * Specifies the command to run when a button is first pressed.
-   *
-   * @param command The pointer to the command to run
-   */
-  virtual void WhenPressed(Command* command);
-
-  /**
-   * Specifies the command to be scheduled while the button is pressed.
-   *
-   * The command will be scheduled repeatedly while the button is pressed and
-   * will be canceled when the button is released.
-   *
-   * @param command The pointer to the command to run
-   */
-  virtual void WhileHeld(Command* command);
-
-  /**
-   * Specifies the command to run when the button is released.
-   *
-   * The command will be scheduled a single time.
-   *
-   * @param command The pointer to the command to run
-   */
-  virtual void WhenReleased(Command* command);
-
-  /**
-   * Cancels the specificed command when the button is pressed.
-   *
-   * @param command The command to be canceled
-   */
-  virtual void CancelWhenPressed(Command* command);
-
-  /**
-   * Toggle the specified command when the button is pressed.
-   *
-   * @param command The command to be toggled
-   */
-  virtual void ToggleWhenPressed(Command* command);
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/ButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/ButtonScheduler.h
deleted file mode 100644
index ec2e35b..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/ButtonScheduler.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class ButtonScheduler {
- public:
-  ButtonScheduler(bool last, Trigger* button, Command* orders);
-  virtual ~ButtonScheduler() = default;
-
-  ButtonScheduler(ButtonScheduler&&) = default;
-  ButtonScheduler& operator=(ButtonScheduler&&) = default;
-
-  virtual void Execute() = 0;
-  void Start();
-
- protected:
-  bool m_pressedLast;
-  Trigger* m_button;
-  Command* m_command;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/CancelButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/CancelButtonScheduler.h
deleted file mode 100644
index 69b8a11..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/CancelButtonScheduler.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/ButtonScheduler.h"
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class CancelButtonScheduler : public ButtonScheduler {
- public:
-  CancelButtonScheduler(bool last, Trigger* button, Command* orders);
-  virtual ~CancelButtonScheduler() = default;
-
-  CancelButtonScheduler(CancelButtonScheduler&&) = default;
-  CancelButtonScheduler& operator=(CancelButtonScheduler&&) = default;
-
-  virtual void Execute();
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/HeldButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/HeldButtonScheduler.h
deleted file mode 100644
index ad4a160..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/HeldButtonScheduler.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/ButtonScheduler.h"
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class HeldButtonScheduler : public ButtonScheduler {
- public:
-  HeldButtonScheduler(bool last, Trigger* button, Command* orders);
-  virtual ~HeldButtonScheduler() = default;
-
-  HeldButtonScheduler(HeldButtonScheduler&&) = default;
-  HeldButtonScheduler& operator=(HeldButtonScheduler&&) = default;
-
-  virtual void Execute();
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/InternalButton.h b/wpilibc/src/main/native/include/frc/buttons/InternalButton.h
deleted file mode 100644
index 430a21e..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/InternalButton.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/Button.h"
-
-namespace frc {
-
-class InternalButton : public Button {
- public:
-  InternalButton() = default;
-  explicit InternalButton(bool inverted);
-  virtual ~InternalButton() = default;
-
-  InternalButton(InternalButton&&) = default;
-  InternalButton& operator=(InternalButton&&) = default;
-
-  void SetInverted(bool inverted);
-  void SetPressed(bool pressed);
-
-  virtual bool Get();
-
- private:
-  bool m_pressed = false;
-  bool m_inverted = false;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/JoystickButton.h b/wpilibc/src/main/native/include/frc/buttons/JoystickButton.h
deleted file mode 100644
index 1b4264f..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/JoystickButton.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/GenericHID.h"
-#include "frc/buttons/Button.h"
-
-namespace frc {
-
-class JoystickButton : public Button {
- public:
-  JoystickButton(GenericHID* joystick, int buttonNumber);
-  virtual ~JoystickButton() = default;
-
-  JoystickButton(JoystickButton&&) = default;
-  JoystickButton& operator=(JoystickButton&&) = default;
-
-  virtual bool Get();
-
- private:
-  GenericHID* m_joystick;
-  int m_buttonNumber;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/NetworkButton.h b/wpilibc/src/main/native/include/frc/buttons/NetworkButton.h
deleted file mode 100644
index fef8065..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/NetworkButton.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <networktables/NetworkTable.h>
-#include <networktables/NetworkTableEntry.h>
-#include <wpi/Twine.h>
-
-#include "frc/buttons/Button.h"
-
-namespace frc {
-
-class NetworkButton : public Button {
- public:
-  NetworkButton(const wpi::Twine& tableName, const wpi::Twine& field);
-  NetworkButton(std::shared_ptr<nt::NetworkTable> table,
-                const wpi::Twine& field);
-  virtual ~NetworkButton() = default;
-
-  NetworkButton(NetworkButton&&) = default;
-  NetworkButton& operator=(NetworkButton&&) = default;
-
-  virtual bool Get();
-
- private:
-  nt::NetworkTableEntry m_entry;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/POVButton.h b/wpilibc/src/main/native/include/frc/buttons/POVButton.h
deleted file mode 100644
index 15c4bf8..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/POVButton.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/GenericHID.h"
-#include "frc/buttons/Button.h"
-
-namespace frc {
-class POVButton : public Button {
- public:
-  /**
-   * Creates a POV button for triggering commands.
-   *
-   * @param joystick The GenericHID object that has the POV
-   * @param angle The desired angle in degrees (e.g. 90, 270)
-   * @param povNumber The POV number (@see GenericHID#GetPOV)
-   */
-  POVButton(GenericHID& joystick, int angle, int povNumber = 0);
-  virtual ~POVButton() = default;
-
-  POVButton(POVButton&&) = default;
-  POVButton& operator=(POVButton&&) = default;
-
-  bool Get() override;
-
- private:
-  GenericHID* m_joystick;
-  int m_angle;
-  int m_povNumber;
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/PressedButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/PressedButtonScheduler.h
deleted file mode 100644
index 0c1fb03..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/PressedButtonScheduler.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/ButtonScheduler.h"
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class PressedButtonScheduler : public ButtonScheduler {
- public:
-  PressedButtonScheduler(bool last, Trigger* button, Command* orders);
-  virtual ~PressedButtonScheduler() = default;
-
-  PressedButtonScheduler(PressedButtonScheduler&&) = default;
-  PressedButtonScheduler& operator=(PressedButtonScheduler&&) = default;
-
-  virtual void Execute();
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
deleted file mode 100644
index 899a483..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/ButtonScheduler.h"
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class ReleasedButtonScheduler : public ButtonScheduler {
- public:
-  ReleasedButtonScheduler(bool last, Trigger* button, Command* orders);
-  virtual ~ReleasedButtonScheduler() = default;
-
-  ReleasedButtonScheduler(ReleasedButtonScheduler&&) = default;
-  ReleasedButtonScheduler& operator=(ReleasedButtonScheduler&&) = default;
-
-  virtual void Execute();
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/ToggleButtonScheduler.h b/wpilibc/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
deleted file mode 100644
index aeb93b3..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/buttons/ButtonScheduler.h"
-
-namespace frc {
-
-class Trigger;
-class Command;
-
-class ToggleButtonScheduler : public ButtonScheduler {
- public:
-  ToggleButtonScheduler(bool last, Trigger* button, Command* orders);
-  virtual ~ToggleButtonScheduler() = default;
-
-  ToggleButtonScheduler(ToggleButtonScheduler&&) = default;
-  ToggleButtonScheduler& operator=(ToggleButtonScheduler&&) = default;
-
-  virtual void Execute();
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/buttons/Trigger.h b/wpilibc/src/main/native/include/frc/buttons/Trigger.h
deleted file mode 100644
index 56700e9..0000000
--- a/wpilibc/src/main/native/include/frc/buttons/Trigger.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <atomic>
-
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class Command;
-
-/**
- * This class provides an easy way to link commands to inputs.
- *
- * It is very easy to link a polled input to a command. For instance, you could
- * link the trigger button of a joystick to a "score" command or an encoder
- * reaching a particular value.
- *
- * It is encouraged that teams write a subclass of Trigger if they want to have
- * something unusual (for instance, if they want to react to the user holding
- * a button while the robot is reading a certain sensor input). For this, they
- * only have to write the {@link Trigger#Get()} method to get the full
- * functionality of the Trigger class.
- */
-class Trigger : public Sendable, public SendableHelper<Trigger> {
- public:
-  Trigger() = default;
-  ~Trigger() override = default;
-
-  Trigger(const Trigger& rhs);
-  Trigger& operator=(const Trigger& rhs);
-  Trigger(Trigger&& rhs);
-  Trigger& operator=(Trigger&& rhs);
-
-  bool Grab();
-  virtual bool Get() = 0;
-  void WhenActive(Command* command);
-  void WhileActive(Command* command);
-  void WhenInactive(Command* command);
-  void CancelWhenActive(Command* command);
-  void ToggleWhenActive(Command* command);
-
-  void InitSendable(SendableBuilder& builder) override;
-
- private:
-  std::atomic_bool m_sendablePressed{false};
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/Command.h b/wpilibc/src/main/native/include/frc/commands/Command.h
deleted file mode 100644
index d40bb00..0000000
--- a/wpilibc/src/main/native/include/frc/commands/Command.h
+++ /dev/null
@@ -1,492 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <string>
-
-#include <wpi/SmallPtrSet.h>
-#include <wpi/Twine.h>
-
-#include "frc/ErrorBase.h"
-#include "frc/commands/Subsystem.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class CommandGroup;
-
-/**
- * The Command class is at the very core of the entire command framework.
- *
- * Every command can be started with a call to Start(). Once a command is
- * started it will call Initialize(), and then will repeatedly call Execute()
- * until the IsFinished() returns true. Once it does,End() will be called.
- *
- * However, if at any point while it is running Cancel() is called, then the
- * command will be stopped and Interrupted() will be called.
- *
- * If a command uses a Subsystem, then it should specify that it does so by
- * calling the Requires() method in its constructor. Note that a Command may
- * have multiple requirements, and Requires() should be called for each one.
- *
- * If a command is running and a new command with shared requirements is
- * started, then one of two things will happen. If the active command is
- * interruptible, then Cancel() will be called and the command will be removed
- * to make way for the new one. If the active command is not interruptible, the
- * other one will not even be started, and the active one will continue
- * functioning.
- *
- * @see CommandGroup
- * @see Subsystem
- */
-class Command : public ErrorBase,
-                public Sendable,
-                public SendableHelper<Command> {
-  friend class CommandGroup;
-  friend class Scheduler;
-
- public:
-  /**
-   * Creates a new command.
-   *
-   * The name of this command will be default.
-   */
-  Command();
-
-  /**
-   * Creates a new command with the given name and no timeout.
-   *
-   * @param name the name for this command
-   */
-  explicit Command(const wpi::Twine& name);
-
-  /**
-   * Creates a new command with the given timeout and a default name.
-   *
-   * @param timeout the time (in seconds) before this command "times out"
-   * @see IsTimedOut()
-   */
-  explicit Command(double timeout);
-
-  /**
-   * Creates a new command with the given timeout and a default name.
-   *
-   * @param subsystem the subsystem that the command requires
-   */
-  explicit Command(Subsystem& subsystem);
-
-  /**
-   * Creates a new command with the given name and timeout.
-   *
-   * @param name    the name of the command
-   * @param timeout the time (in seconds) before this command "times out"
-   * @see IsTimedOut()
-   */
-  Command(const wpi::Twine& name, double timeout);
-
-  /**
-   * Creates a new command with the given name and timeout.
-   *
-   * @param name      the name of the command
-   * @param subsystem the subsystem that the command requires
-   */
-  Command(const wpi::Twine& name, Subsystem& subsystem);
-
-  /**
-   * Creates a new command with the given name and timeout.
-   *
-   * @param timeout   the time (in seconds) before this command "times out"
-   * @param subsystem the subsystem that the command requires @see IsTimedOut()
-   */
-  Command(double timeout, Subsystem& subsystem);
-
-  /**
-   * Creates a new command with the given name and timeout.
-   *
-   * @param name      the name of the command
-   * @param timeout   the time (in seconds) before this command "times out"
-   * @param subsystem the subsystem that the command requires @see IsTimedOut()
-   */
-  Command(const wpi::Twine& name, double timeout, Subsystem& subsystem);
-
-  ~Command() override = default;
-
-  Command(Command&&) = default;
-  Command& operator=(Command&&) = default;
-
-  /**
-   * Returns the time since this command was initialized (in seconds).
-   *
-   * This function will work even if there is no specified timeout.
-   *
-   * @return the time since this command was initialized (in seconds).
-   */
-  double TimeSinceInitialized() const;
-
-  /**
-   * This method specifies that the given Subsystem is used by this command.
-   *
-   * This method is crucial to the functioning of the Command System in general.
-   *
-   * Note that the recommended way to call this method is in the constructor.
-   *
-   * @param subsystem The Subsystem required
-   * @see Subsystem
-   */
-  void Requires(Subsystem* s);
-
-  /**
-   * Starts up the command. Gets the command ready to start.
-   *
-   * Note that the command will eventually start, however it will not
-   * necessarily do so immediately, and may in fact be canceled before
-   * initialize is even called.
-   */
-  void Start();
-
-  /**
-   * The run method is used internally to actually run the commands.
-   *
-   * @return Whether or not the command should stay within the Scheduler.
-   */
-  bool Run();
-
-  /**
-   * This will cancel the current command.
-   *
-   * This will cancel the current command eventually. It can be called multiple
-   * times. And it can be called when the command is not running. If the command
-   * is running though, then the command will be marked as canceled and
-   * eventually removed.
-   *
-   * A command can not be canceled if it is a part of a command group, you must
-   * cancel the command group instead.
-   */
-  void Cancel();
-
-  /**
-   * Returns whether or not the command is running.
-   *
-   * This may return true even if the command has just been canceled, as it may
-   * not have yet called Interrupted().
-   *
-   * @return whether or not the command is running
-   */
-  bool IsRunning() const;
-
-  /**
-   * Returns whether or not the command has been initialized.
-   *
-   * @return whether or not the command has been initialized.
-   */
-  bool IsInitialized() const;
-
-  /**
-   * Returns whether or not the command has completed running.
-   *
-   * @return whether or not the command has completed running.
-   */
-  bool IsCompleted() const;
-
-  /**
-   * Returns whether or not this has been canceled.
-   *
-   * @return whether or not this has been canceled
-   */
-  bool IsCanceled() const;
-
-  /**
-   * Returns whether or not this command can be interrupted.
-   *
-   * @return whether or not this command can be interrupted
-   */
-  bool IsInterruptible() const;
-
-  /**
-   * Sets whether or not this command can be interrupted.
-   *
-   * @param interruptible whether or not this command can be interrupted
-   */
-  void SetInterruptible(bool interruptible);
-
-  /**
-   * Checks if the command requires the given Subsystem.
-   *
-   * @param system the system
-   * @return whether or not the subsystem is required (false if given nullptr)
-   */
-  bool DoesRequire(Subsystem* subsystem) const;
-
-  using SubsystemSet = wpi::SmallPtrSetImpl<Subsystem*>;
-
-  /**
-   * Returns the requirements (as an std::set of Subsystem pointers) of this
-   * command.
-   *
-   * @return The requirements (as an std::set of Subsystem pointers) of this
-   *         command
-   */
-  const SubsystemSet& GetRequirements() const;
-
-  /**
-   * Returns the CommandGroup that this command is a part of.
-   *
-   * Will return null if this Command is not in a group.
-   *
-   * @return The CommandGroup that this command is a part of (or null if not in
-   *         group)
-   */
-  CommandGroup* GetGroup() const;
-
-  /**
-   * Sets whether or not this Command should run when the robot is disabled.
-   *
-   * By default a command will not run when the robot is disabled, and will in
-   * fact be canceled.
-   *
-   * @param run Whether this command should run when the robot is disabled.
-   */
-  void SetRunWhenDisabled(bool run);
-
-  /**
-   * Returns whether or not this Command will run when the robot is disabled, or
-   * if it will cancel itself.
-   *
-   * @return Whether this Command will run when the robot is disabled, or if it
-   * will cancel itself.
-   */
-  bool WillRunWhenDisabled() const;
-
-  /**
-   * Get the ID (sequence number) for this command.
-   *
-   * The ID is a unique sequence number that is incremented for each command.
-   *
-   * @return The ID of this command
-   */
-  int GetID() const;
-
- protected:
-  /**
-   * Sets the timeout of this command.
-   *
-   * @param timeout the timeout (in seconds)
-   * @see IsTimedOut()
-   */
-  void SetTimeout(double timeout);
-
-  /**
-   * Returns whether or not the TimeSinceInitialized() method returns a number
-   * which is greater than or equal to the timeout for the command.
-   *
-   * If there is no timeout, this will always return false.
-   *
-   * @return whether the time has expired
-   */
-  bool IsTimedOut() const;
-
-  /**
-   * If changes are locked, then this will generate a CommandIllegalUse error.
-   *
-   * @param message The message to report on error (it is appended by a default
-   *                message)
-   * @return True if assert passed, false if assert failed.
-   */
-  bool AssertUnlocked(const std::string& message);
-
-  /**
-   * Sets the parent of this command. No actual change is made to the group.
-   *
-   * @param parent the parent
-   */
-  void SetParent(CommandGroup* parent);
-
-  /**
-   * Returns whether the command has a parent.
-   *
-   * @param True if the command has a parent.
-   */
-  bool IsParented() const;
-
-  /**
-   * Clears list of subsystem requirements.
-   *
-   * This is only used by ConditionalCommand so cancelling the chosen command
-   * works properly in CommandGroup.
-   */
-  void ClearRequirements();
-
-  /**
-   * The initialize method is called the first time this Command is run after
-   * being started.
-   */
-  virtual void Initialize();
-
-  /**
-   * The execute method is called repeatedly until this Command either finishes
-   * or is canceled.
-   */
-  virtual void Execute();
-
-  /**
-   * Returns whether this command is finished.
-   *
-   * If it is, then the command will be removed and End() will be called.
-   *
-   * It may be useful for a team to reference the IsTimedOut() method for
-   * time-sensitive commands.
-   *
-   * Returning false will result in the command never ending automatically.
-   * It may still be cancelled manually or interrupted by another command.
-   * Returning true will result in the command executing once and finishing
-   * immediately. We recommend using InstantCommand for this.
-   *
-   * @return Whether this command is finished.
-   * @see IsTimedOut()
-   */
-  virtual bool IsFinished() = 0;
-
-  /**
-   * Called when the command ended peacefully.
-   *
-   * This is where you may want to wrap up loose ends, like shutting off a motor
-   * that was being used in the command.
-   */
-  virtual void End();
-
-  /**
-   * Called when the command ends because somebody called Cancel() or another
-   * command shared the same requirements as this one, and booted it out.
-   *
-   * This is where you may want to wrap up loose ends, like shutting off a motor
-   * that was being used in the command.
-   *
-   * Generally, it is useful to simply call the End() method within this method,
-   * as done here.
-   */
-  virtual void Interrupted();
-
-  virtual void _Initialize();
-  virtual void _Interrupted();
-  virtual void _Execute();
-  virtual void _End();
-
-  /**
-   * This works like Cancel(), except that it doesn't throw an exception if it
-   * is a part of a command group.
-   *
-   * Should only be called by the parent command group.
-   */
-  virtual void _Cancel();
-
-  friend class ConditionalCommand;
-
-  /**
-   * Gets the name of this Command.
-   *
-   * @return Name
-   */
-  std::string GetName() const;
-
-  /**
-   * Sets the name of this Command.
-   *
-   * @param name name
-   */
-  void SetName(const wpi::Twine& name);
-
-  /**
-   * Gets the subsystem name of this Command.
-   *
-   * @return Subsystem name
-   */
-  std::string GetSubsystem() const;
-
-  /**
-   * Sets the subsystem name of this Command.
-   *
-   * @param subsystem subsystem name
-   */
-  void SetSubsystem(const wpi::Twine& subsystem);
-
- private:
-  /**
-   * Prevents further changes from being made.
-   */
-  void LockChanges();
-
-  /**
-   * Called when the command has been removed.
-   *
-   * This will call Interrupted() or End().
-   */
-  void Removed();
-
-  /**
-   * This is used internally to mark that the command has been started.
-   *
-   * The lifecycle of a command is:
-   *
-   * StartRunning() is called. Run() is called (multiple times potentially).
-   * Removed() is called.
-   *
-   * It is very important that StartRunning() and Removed() be called in order
-   * or some assumptions of the code will be broken.
-   */
-  void StartRunning();
-
-  /**
-   * Called to indicate that the timer should start.
-   *
-   * This is called right before Initialize() is, inside the Run() method.
-   */
-  void StartTiming();
-
-  // The time since this command was initialized
-  double m_startTime = -1;
-
-  // The time (in seconds) before this command "times out" (-1 if no timeout)
-  double m_timeout;
-
-  // Whether or not this command has been initialized
-  bool m_initialized = false;
-
-  // The requirements (or null if no requirements)
-  wpi::SmallPtrSet<Subsystem*, 4> m_requirements;
-
-  // Whether or not it is running
-  bool m_running = false;
-
-  // Whether or not it is interruptible
-  bool m_interruptible = true;
-
-  // Whether or not it has been canceled
-  bool m_canceled = false;
-
-  // Whether or not it has been locked
-  bool m_locked = false;
-
-  // Whether this command should run when the robot is disabled
-  bool m_runWhenDisabled = false;
-
-  // 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;
-
- public:
-  void InitSendable(SendableBuilder& builder) override;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/CommandGroup.h b/wpilibc/src/main/native/include/frc/commands/CommandGroup.h
deleted file mode 100644
index 690ae6d..0000000
--- a/wpilibc/src/main/native/include/frc/commands/CommandGroup.h
+++ /dev/null
@@ -1,180 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <vector>
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-#include "frc/commands/CommandGroupEntry.h"
-
-namespace frc {
-
-/**
- * A CommandGroup is a list of commands which are executed in sequence.
- *
- * Commands in a CommandGroup are added using the AddSequential() method and are
- * called sequentially. CommandGroups are themselves Commands and can be given
- * to other CommandGroups.
- *
- * CommandGroups will carry all of the requirements of their Command
- * subcommands. Additional requirements can be specified by calling Requires()
- * normally in the constructor.
- *
- * CommandGroups can also execute commands in parallel, simply by adding them
- * using AddParallel().
- *
- * @see Command
- * @see Subsystem
- */
-class CommandGroup : public Command {
- public:
-  CommandGroup() = default;
-
-  /**
-   * Creates a new CommandGroup with the given name.
-   *
-   * @param name The name for this command group
-   */
-  explicit CommandGroup(const wpi::Twine& name);
-
-  virtual ~CommandGroup() = default;
-
-  CommandGroup(CommandGroup&&) = default;
-  CommandGroup& operator=(CommandGroup&&) = default;
-
-  /**
-   * Adds a new Command to the group. The Command will be started after all the
-   * previously added Commands.
-   *
-   * Note that any requirements the given Command has will be added to the
-   * group. For this reason, a Command's requirements can not be changed after
-   * being added to a group.
-   *
-   * It is recommended that this method be called in the constructor.
-   *
-   * @param command The Command to be added
-   */
-  void AddSequential(Command* command);
-
-  /**
-   * Adds a new Command to the group with a given timeout. The Command will be
-   * started after all the previously added commands.
-   *
-   * Once the Command is started, it will be run until it finishes or the time
-   * expires, whichever is sooner.  Note that the given Command will have no
-   * knowledge that it is on a timer.
-   *
-   * Note that any requirements the given Command has will be added to the
-   * group. For this reason, a Command's requirements can not be changed after
-   * being added to a group.
-   *
-   * It is recommended that this method be called in the constructor.
-   *
-   * @param command The Command to be added
-   * @param timeout The timeout (in seconds)
-   */
-  void AddSequential(Command* command, double timeout);
-
-  /**
-   * Adds a new child Command to the group. The Command will be started after
-   * all the previously added Commands.
-   *
-   * Instead of waiting for the child to finish, a CommandGroup will have it run
-   * at the same time as the subsequent Commands. The child will run until
-   * either it finishes, a new child with conflicting requirements is started,
-   * or the main sequence runs a Command with conflicting requirements. In the
-   * latter two cases, the child will be canceled even if it says it can't be
-   * interrupted.
-   *
-   * Note that any requirements the given Command has will be added to the
-   * group. For this reason, a Command's requirements can not be changed after
-   * being added to a group.
-   *
-   * It is recommended that this method be called in the constructor.
-   *
-   * @param command The command to be added
-   */
-  void AddParallel(Command* command);
-
-  /**
-   * Adds a new child Command to the group with the given timeout. The Command
-   * will be started after all the previously added Commands.
-   *
-   * Once the Command is started, it will run until it finishes, is interrupted,
-   * or the time expires, whichever is sooner. Note that the given Command will
-   * have no knowledge that it is on a timer.
-   *
-   * Instead of waiting for the child to finish, a CommandGroup will have it run
-   * at the same time as the subsequent Commands. The child will run until
-   * either it finishes, the timeout expires, a new child with conflicting
-   * requirements is started, or the main sequence runs a Command with
-   * conflicting requirements. In the latter two cases, the child will be
-   * canceled even if it says it can't be interrupted.
-   *
-   * Note that any requirements the given Command has will be added to the
-   * group. For this reason, a Command's requirements can not be changed after
-   * being added to a group.
-   *
-   * It is recommended that this method be called in the constructor.
-   *
-   * @param command The command to be added
-   * @param timeout The timeout (in seconds)
-   */
-  void AddParallel(Command* command, double timeout);
-
-  bool IsInterruptible() const;
-
-  int GetSize() const;
-
- protected:
-  /**
-   * Can be overridden by teams.
-   */
-  virtual void Initialize();
-
-  /**
-   * Can be overridden by teams.
-   */
-  virtual void Execute();
-
-  /**
-   * Can be overridden by teams.
-   */
-  virtual bool IsFinished();
-
-  /**
-   * Can be overridden by teams.
-   */
-  virtual void End();
-
-  /**
-   * Can be overridden by teams.
-   */
-  virtual void Interrupted();
-
-  virtual void _Initialize();
-  virtual void _Execute();
-  virtual void _End();
-  virtual void _Interrupted();
-
- private:
-  void CancelConflicts(Command* command);
-
-  // The commands in this group (stored in entries)
-  std::vector<CommandGroupEntry> m_commands;
-
-  // The active children in this group (stored in entries)
-  std::vector<CommandGroupEntry*> m_children;
-
-  // The current command, -1 signifies that none have been run
-  int m_currentCommandIndex = -1;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/CommandGroupEntry.h b/wpilibc/src/main/native/include/frc/commands/CommandGroupEntry.h
deleted file mode 100644
index 2de1e43..0000000
--- a/wpilibc/src/main/native/include/frc/commands/CommandGroupEntry.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace frc {
-
-class Command;
-
-class CommandGroupEntry {
- public:
-  enum Sequence {
-    kSequence_InSequence,
-    kSequence_BranchPeer,
-    kSequence_BranchChild
-  };
-
-  CommandGroupEntry() = default;
-  CommandGroupEntry(Command* command, Sequence state, double timeout = -1.0);
-
-  CommandGroupEntry(CommandGroupEntry&&) = default;
-  CommandGroupEntry& operator=(CommandGroupEntry&&) = default;
-
-  bool IsTimedOut() const;
-
-  double m_timeout = -1.0;
-  Command* m_command = nullptr;
-  Sequence m_state = kSequence_InSequence;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h b/wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h
deleted file mode 100644
index f5cd738..0000000
--- a/wpilibc/src/main/native/include/frc/commands/ConditionalCommand.h
+++ /dev/null
@@ -1,84 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-/**
- * A ConditionalCommand is a Command that starts one of two commands.
- *
- * 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 onFalse, the occurrence of that condition
- * will be a no-op.
- *
- * A ConditionalCommand will require the superset of subsystems of the onTrue
- * and onFalse commands.
- *
- * @see Command
- * @see Scheduler
- */
-class ConditionalCommand : public Command {
- public:
-  /**
-   * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
-   *
-   * @param onTrue  The Command to execute if Condition() returns true
-   * @param onFalse The Command to execute if Condition() returns false
-   */
-  explicit ConditionalCommand(Command* onTrue, Command* onFalse = nullptr);
-
-  /**
-   * Creates a new ConditionalCommand with given onTrue and onFalse Commands.
-   *
-   * @param name    The name for this command group
-   * @param onTrue  The Command to execute if Condition() returns true
-   * @param onFalse The Command to execute if Condition() returns false
-   */
-  ConditionalCommand(const wpi::Twine& name, Command* onTrue,
-                     Command* onFalse = nullptr);
-
-  virtual ~ConditionalCommand() = default;
-
-  ConditionalCommand(ConditionalCommand&&) = default;
-  ConditionalCommand& operator=(ConditionalCommand&&) = default;
-
- protected:
-  /**
-   * The Condition to test to determine which Command to run.
-   *
-   * @return true if m_onTrue should be run or false if m_onFalse should be run.
-   */
-  virtual bool Condition() = 0;
-
-  void _Initialize() override;
-  void _Cancel() override;
-  bool IsFinished() override;
-  void _Interrupted() override;
-
- private:
-  // The Command to execute if Condition() returns true
-  Command* m_onTrue;
-
-  // The Command to execute if Condition() returns false
-  Command* m_onFalse;
-
-  // Stores command chosen by condition
-  Command* m_chosenCommand = nullptr;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/InstantCommand.h b/wpilibc/src/main/native/include/frc/commands/InstantCommand.h
deleted file mode 100644
index 987dc18..0000000
--- a/wpilibc/src/main/native/include/frc/commands/InstantCommand.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <functional>
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-#include "frc/commands/Subsystem.h"
-
-namespace frc {
-
-/**
- * This command will execute once, then finish immediately afterward.
- *
- * Subclassing InstantCommand is shorthand for returning true from IsFinished().
- */
-class InstantCommand : public Command {
- public:
-  /**
-   * Creates a new InstantCommand with the given name.
-   *
-   * @param name The name for this command
-   */
-  explicit InstantCommand(const wpi::Twine& name);
-
-  /**
-   * Creates a new InstantCommand with the given requirement.
-   *
-   * @param subsystem The subsystem that the command requires
-   */
-  explicit InstantCommand(Subsystem& subsystem);
-
-  /**
-   * Creates a new InstantCommand with the given name.
-   *
-   * @param name      The name for this command
-   * @param subsystem The subsystem that the command requires
-   */
-  InstantCommand(const wpi::Twine& name, Subsystem& subsystem);
-
-  /**
-   * Create a command that calls the given function when run.
-   *
-   * @param func The function to run when Initialize() is run.
-   */
-  explicit InstantCommand(std::function<void()> func);
-
-  /**
-   * Create a command that calls the given function when run.
-   *
-   * @param subsystem The subsystems that this command runs on.
-   * @param func      The function to run when Initialize() is run.
-   */
-  InstantCommand(Subsystem& subsystem, std::function<void()> func);
-
-  /**
-   * Create a command that calls the given function when run.
-   *
-   * @param name   The name of the command.
-   * @param func   The function to run when Initialize() is run.
-   */
-  InstantCommand(const wpi::Twine& name, std::function<void()> func);
-
-  /**
-   * Create a command that calls the given function when run.
-   *
-   * @param name      The name of the command.
-   * @param subsystem The subsystems that this command runs on.
-   * @param func      The function to run when Initialize() is run.
-   */
-  InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
-                 std::function<void()> func);
-
-  InstantCommand() = default;
-  virtual ~InstantCommand() = default;
-
-  InstantCommand(InstantCommand&&) = default;
-  InstantCommand& operator=(InstantCommand&&) = default;
-
- protected:
-  std::function<void()> m_func = nullptr;
-  void _Initialize() override;
-  bool IsFinished() override;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/PIDCommand.h b/wpilibc/src/main/native/include/frc/commands/PIDCommand.h
deleted file mode 100644
index 02f1710..0000000
--- a/wpilibc/src/main/native/include/frc/commands/PIDCommand.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <wpi/Twine.h>
-
-#include "frc/PIDController.h"
-#include "frc/PIDOutput.h"
-#include "frc/PIDSource.h"
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-class PIDCommand : public Command, public PIDOutput, public PIDSource {
- public:
-  PIDCommand(const wpi::Twine& name, double p, double i, double d);
-  PIDCommand(const wpi::Twine& name, double p, double i, double d,
-             double period);
-  PIDCommand(const wpi::Twine& name, double p, double i, double d, double f,
-             double period);
-  PIDCommand(double p, double i, double d);
-  PIDCommand(double p, double i, double d, double period);
-  PIDCommand(double p, double i, double d, double f, double period);
-  PIDCommand(const wpi::Twine& name, double p, double i, double d,
-             Subsystem& subsystem);
-  PIDCommand(const wpi::Twine& name, double p, double i, double d,
-             double period, Subsystem& subsystem);
-  PIDCommand(const wpi::Twine& name, double p, double i, double d, double f,
-             double period, Subsystem& subsystem);
-  PIDCommand(double p, double i, double d, Subsystem& subsystem);
-  PIDCommand(double p, double i, double d, double period, Subsystem& subsystem);
-  PIDCommand(double p, double i, double d, double f, double period,
-             Subsystem& subsystem);
-  virtual ~PIDCommand() = default;
-
-  PIDCommand(PIDCommand&&) = default;
-  PIDCommand& operator=(PIDCommand&&) = default;
-
-  void SetSetpointRelative(double deltaSetpoint);
-
-  // PIDOutput interface
-  void PIDWrite(double output) override;
-
-  // PIDSource interface
-  double PIDGet() override;
-
- protected:
-  std::shared_ptr<PIDController> GetPIDController() const;
-  void _Initialize() override;
-  void _Interrupted() override;
-  void _End() override;
-  void SetSetpoint(double setpoint);
-  double GetSetpoint() const;
-  double GetPosition();
-
-  virtual double ReturnPIDInput() = 0;
-  virtual void UsePIDOutput(double output) = 0;
-
- private:
-  // The internal PIDController
-  std::shared_ptr<PIDController> m_controller;
-
- public:
-  void InitSendable(SendableBuilder& builder) override;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/PIDSubsystem.h b/wpilibc/src/main/native/include/frc/commands/PIDSubsystem.h
deleted file mode 100644
index 1965492..0000000
--- a/wpilibc/src/main/native/include/frc/commands/PIDSubsystem.h
+++ /dev/null
@@ -1,236 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <wpi/Twine.h>
-
-#include "frc/PIDController.h"
-#include "frc/PIDOutput.h"
-#include "frc/PIDSource.h"
-#include "frc/commands/Subsystem.h"
-
-namespace frc {
-
-/**
- * This class is designed to handle the case where there is a Subsystem which
- * uses a single PIDController almost constantly (for instance, an elevator
- * which attempts to stay at a constant height).
- *
- * It provides some convenience methods to run an internal PIDController. It
- * also allows access to the internal PIDController in order to give total
- * control to the programmer.
- */
-class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource {
- public:
-  /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
-   *
-   * @param name the name
-   * @param p    the proportional value
-   * @param i    the integral value
-   * @param d    the derivative value
-   */
-  PIDSubsystem(const wpi::Twine& name, double p, double i, double d);
-
-  /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
-   *
-   * @param name the name
-   * @param p    the proportional value
-   * @param i    the integral value
-   * @param d    the derivative value
-   * @param f    the feedforward value
-   */
-  PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f);
-
-  /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
-   *
-   * It will also space the time between PID loop calculations to be equal to
-   * the given period.
-   *
-   * @param name   the name
-   * @param p      the proportional value
-   * @param i      the integral value
-   * @param d      the derivative value
-   * @param f      the feedfoward value
-   * @param period the time (in seconds) between calculations
-   */
-  PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f,
-               double period);
-
-  /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
-   *
-   * It will use the class name as its name.
-   *
-   * @param p the proportional value
-   * @param i the integral value
-   * @param d the derivative value
-   */
-  PIDSubsystem(double p, double i, double d);
-
-  /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
-   *
-   * It will use the class name as its name.
-   *
-   * @param p the proportional value
-   * @param i the integral value
-   * @param d the derivative value
-   * @param f the feedforward value
-   */
-  PIDSubsystem(double p, double i, double d, double f);
-
-  /**
-   * Instantiates a PIDSubsystem that will use the given P, I, and D values.
-   *
-   * It will use the class name as its name. It will also space the time
-   * between PID loop calculations to be equal to the given period.
-   *
-   * @param p      the proportional value
-   * @param i      the integral value
-   * @param d      the derivative value
-   * @param f      the feedforward value
-   * @param period the time (in seconds) between calculations
-   */
-  PIDSubsystem(double p, double i, double d, double f, double period);
-
-  ~PIDSubsystem() override = default;
-
-  PIDSubsystem(PIDSubsystem&&) = default;
-  PIDSubsystem& operator=(PIDSubsystem&&) = default;
-
-  /**
-   * Enables the internal PIDController.
-   */
-  void Enable();
-
-  /**
-   * Disables the internal PIDController.
-   */
-  void Disable();
-
-  // PIDOutput interface
-  void PIDWrite(double output) override;
-
-  // PIDSource interface
-
-  double PIDGet() override;
-
-  /**
-   * Sets the setpoint to the given value.
-   *
-   * If SetRange() was called, then the given setpoint will be trimmed to fit
-   * within the range.
-   *
-   * @param setpoint the new setpoint
-   */
-  void SetSetpoint(double setpoint);
-
-  /**
-   * Adds the given value to the setpoint.
-   *
-   * If SetRange() was used, then the bounds will still be honored by this
-   * method.
-   *
-   * @param deltaSetpoint the change in the setpoint
-   */
-  void SetSetpointRelative(double deltaSetpoint);
-
-  /**
-   * Sets the maximum and minimum values expected from the input.
-   *
-   * @param minimumInput the minimum value expected from the input
-   * @param maximumInput the maximum value expected from the output
-   */
-  void SetInputRange(double minimumInput, double maximumInput);
-
-  /**
-   * Sets the maximum and minimum values to write.
-   *
-   * @param minimumOutput the minimum value to write to the output
-   * @param maximumOutput the maximum value to write to the output
-   */
-  void SetOutputRange(double minimumOutput, double maximumOutput);
-
-  /**
-   * Return the current setpoint.
-   *
-   * @return The current setpoint
-   */
-  double GetSetpoint();
-
-  /**
-   * Returns the current position.
-   *
-   * @return the current position
-   */
-  double GetPosition();
-
-  /**
-   * Returns the current rate.
-   *
-   * @return the current rate
-   */
-  double GetRate();
-
-  /**
-   * Set the absolute error which is considered tolerable for use with
-   * OnTarget.
-   *
-   * @param absValue absolute error which is tolerable
-   */
-  virtual void SetAbsoluteTolerance(double absValue);
-
-  /**
-   * Set the percentage error which is considered tolerable for use with
-   * OnTarget().
-   *
-   * @param percent percentage error which is tolerable
-   */
-  virtual void SetPercentTolerance(double percent);
-
-  /**
-   * Return true if the error is within the percentage of the total input range,
-   * determined by SetTolerance().
-   *
-   * This asssumes that the maximum and minimum input were set using SetInput().
-   * Use OnTarget() in the IsFinished() method of commands that use this
-   * subsystem.
-   *
-   * Currently this just reports on target as the actual value passes through
-   * the setpoint. Ideally it should be based on being within the tolerance for
-   * some period of time.
-   *
-   * @return True if the error is within the percentage tolerance of the input
-   *         range
-   */
-  virtual bool OnTarget() const;
-
- protected:
-  /**
-   * Returns the PIDController used by this PIDSubsystem.
-   *
-   * Use this if you would like to fine tune the PID loop.
-   *
-   * @return The PIDController used by this PIDSubsystem
-   */
-  std::shared_ptr<PIDController> GetPIDController();
-
-  virtual double ReturnPIDInput() = 0;
-  virtual void UsePIDOutput(double output) = 0;
-
- private:
-  // The internal PIDController
-  std::shared_ptr<PIDController> m_controller;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/PrintCommand.h b/wpilibc/src/main/native/include/frc/commands/PrintCommand.h
deleted file mode 100644
index 13156c9..0000000
--- a/wpilibc/src/main/native/include/frc/commands/PrintCommand.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <string>
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/InstantCommand.h"
-
-namespace frc {
-
-class PrintCommand : public InstantCommand {
- public:
-  explicit PrintCommand(const wpi::Twine& message);
-  virtual ~PrintCommand() = default;
-
-  PrintCommand(PrintCommand&&) = default;
-  PrintCommand& operator=(PrintCommand&&) = default;
-
- protected:
-  virtual void Initialize();
-
- private:
-  std::string m_message;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/Scheduler.h b/wpilibc/src/main/native/include/frc/commands/Scheduler.h
deleted file mode 100644
index d15ea39..0000000
--- a/wpilibc/src/main/native/include/frc/commands/Scheduler.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class ButtonScheduler;
-class Command;
-class Subsystem;
-
-class Scheduler : public ErrorBase,
-                  public Sendable,
-                  public SendableHelper<Scheduler> {
- public:
-  /**
-   * Returns the Scheduler, creating it if one does not exist.
-   *
-   * @return the Scheduler
-   */
-  static Scheduler* GetInstance();
-
-  /**
-   * Add a command to be scheduled later.
-   *
-   * In any pass through the scheduler, all commands are added to the additions
-   * list, then at the end of the pass, they are all scheduled.
-   *
-   * @param command The command to be scheduled
-   */
-  void AddCommand(Command* command);
-
-  void AddButton(ButtonScheduler* button);
-
-  /**
-   * Registers a Subsystem to this Scheduler, so that the Scheduler might know
-   * if a default Command needs to be run.
-   *
-   * All Subsystems should call this.
-   *
-   * @param system the system
-   */
-  void RegisterSubsystem(Subsystem* subsystem);
-
-  /**
-   * Runs a single iteration of the loop.
-   *
-   * This method should be called often in order to have a functioning
-   * Command system. The loop has five stages:
-   *
-   * <ol>
-   *   <li>Poll the Buttons</li>
-   *   <li>Execute/Remove the Commands</li>
-   *   <li>Send values to SmartDashboard</li>
-   *   <li>Add Commands</li>
-   *   <li>Add Defaults</li>
-   * </ol>
-   */
-  void Run();
-
-  /**
-   * Removes the Command from the Scheduler.
-   *
-   * @param command the command to remove
-   */
-  void Remove(Command* command);
-
-  void RemoveAll();
-
-  /**
-   * Completely resets the scheduler. Undefined behavior if running.
-   */
-  void ResetAll();
-
-  void SetEnabled(bool enabled);
-
-  void InitSendable(SendableBuilder& builder) override;
-
- private:
-  Scheduler();
-  ~Scheduler() override;
-
-  Scheduler(Scheduler&&) = default;
-  Scheduler& operator=(Scheduler&&) = default;
-
-  struct Impl;
-  std::unique_ptr<Impl> m_impl;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/StartCommand.h b/wpilibc/src/main/native/include/frc/commands/StartCommand.h
deleted file mode 100644
index 4f0b676..0000000
--- a/wpilibc/src/main/native/include/frc/commands/StartCommand.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/commands/InstantCommand.h"
-
-namespace frc {
-
-class StartCommand : public InstantCommand {
- public:
-  explicit StartCommand(Command* commandToStart);
-  virtual ~StartCommand() = default;
-
-  StartCommand(StartCommand&&) = default;
-  StartCommand& operator=(StartCommand&&) = default;
-
- protected:
-  virtual void Initialize();
-
- private:
-  Command* m_commandToFork;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/Subsystem.h b/wpilibc/src/main/native/include/frc/commands/Subsystem.h
deleted file mode 100644
index 2637dd0..0000000
--- a/wpilibc/src/main/native/include/frc/commands/Subsystem.h
+++ /dev/null
@@ -1,199 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <string>
-
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
-
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class Command;
-
-class Subsystem : public ErrorBase,
-                  public Sendable,
-                  public SendableHelper<Subsystem> {
-  friend class Scheduler;
-
- public:
-  /**
-   * Creates a subsystem with the given name.
-   *
-   * @param name the name of the subsystem
-   */
-  explicit Subsystem(const wpi::Twine& name);
-
-  Subsystem(Subsystem&&) = default;
-  Subsystem& operator=(Subsystem&&) = default;
-
-  /**
-   * Sets the default command. If this is not called or is called with null,
-   * then there will be no default command for the subsystem.
-   *
-   * <b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the
-   * subsystem is a singleton.
-   *
-   * @param command the default command (or null if there should be none)
-   */
-  void SetDefaultCommand(Command* command);
-
-  /**
-   * Returns the default command (or null if there is none).
-   *
-   * @return the default command
-   */
-  Command* GetDefaultCommand();
-
-  /**
-   * Returns the default command name, or empty string is there is none.
-   *
-   * @return the default command name
-   */
-  wpi::StringRef GetDefaultCommandName();
-
-  /**
-   * Sets the current command.
-   *
-   * @param command the new current command
-   */
-  void SetCurrentCommand(Command* command);
-
-  /**
-   * Returns the command which currently claims this subsystem.
-   *
-   * @return the command which currently claims this subsystem
-   */
-  Command* GetCurrentCommand() const;
-
-  /**
-   * Returns the current command name, or empty string if no current command.
-   *
-   * @return the current command name
-   */
-  wpi::StringRef GetCurrentCommandName() const;
-
-  /**
-   * When the run method of the scheduler is called this method will be called.
-   */
-  virtual void Periodic();
-
-  /**
-   * Initialize the default command for this subsystem.
-   *
-   * This is meant to be the place to call SetDefaultCommand in a subsystem and
-   * will be called on all the subsystems by the CommandBase method before the
-   * program starts running by using the list of all registered Subsystems
-   * inside the Scheduler.
-   *
-   * This should be overridden by a Subsystem that has a default Command
-   */
-  virtual void InitDefaultCommand();
-
-  /**
-   * Gets the name of this Subsystem.
-   *
-   * @return Name
-   */
-  std::string GetName() const;
-
-  /**
-   * Sets the name of this Subsystem.
-   *
-   * @param name name
-   */
-  void SetName(const wpi::Twine& name);
-
-  /**
-   * Gets the subsystem name of this Subsystem.
-   *
-   * @return Subsystem name
-   */
-  std::string GetSubsystem() const;
-
-  /**
-   * Sets the subsystem name of this Subsystem.
-   *
-   * @param subsystem subsystem name
-   */
-  void SetSubsystem(const wpi::Twine& subsystem);
-
-  /**
-   * Associate a Sendable with this Subsystem.
-   * Also update the child's name.
-   *
-   * @param name name to give child
-   * @param child sendable
-   */
-  void AddChild(const wpi::Twine& name, std::shared_ptr<Sendable> child);
-
-  /**
-   * Associate a Sendable with this Subsystem.
-   * Also update the child's name.
-   *
-   * @param name name to give child
-   * @param child sendable
-   */
-  void AddChild(const wpi::Twine& name, Sendable* child);
-
-  /**
-   * Associate a Sendable with this Subsystem.
-   * Also update the child's name.
-   *
-   * @param name name to give child
-   * @param child sendable
-   */
-  void AddChild(const wpi::Twine& name, Sendable& child);
-
-  /**
-   * Associate a {@link Sendable} with this Subsystem.
-   *
-   * @param child sendable
-   */
-  void AddChild(std::shared_ptr<Sendable> child);
-
-  /**
-   * Associate a {@link Sendable} with this Subsystem.
-   *
-   * @param child sendable
-   */
-  void AddChild(Sendable* child);
-
-  /**
-   * Associate a {@link Sendable} with this Subsystem.
-   *
-   * @param child sendable
-   */
-  void AddChild(Sendable& child);
-
- private:
-  /**
-   * Call this to alert Subsystem that the current command is actually the
-   * command.
-   *
-   * Sometimes, the Subsystem is told that it has no command while the Scheduler
-   * is going through the loop, only to be soon after given a new one. This will
-   * avoid that situation.
-   */
-  void ConfirmCommand();
-
-  Command* m_currentCommand = nullptr;
-  bool m_currentCommandChanged = true;
-  Command* m_defaultCommand = nullptr;
-  bool m_initializedDefaultCommand = false;
-
- public:
-  void InitSendable(SendableBuilder& builder) override;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/TimedCommand.h b/wpilibc/src/main/native/include/frc/commands/TimedCommand.h
deleted file mode 100644
index d2be010..0000000
--- a/wpilibc/src/main/native/include/frc/commands/TimedCommand.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-/**
- * A TimedCommand will wait for a timeout before finishing.
- *
- * TimedCommand is used to execute a command for a given amount of time.
- */
-class TimedCommand : public Command {
- public:
-  /**
-   * Creates a new TimedCommand with the given name and timeout.
-   *
-   * @param name    the name of the command
-   * @param timeout the time (in seconds) before this command "times out"
-   */
-  TimedCommand(const wpi::Twine& name, double timeout);
-
-  /**
-   * Creates a new WaitCommand with the given timeout.
-   *
-   * @param timeout the time (in seconds) before this command "times out"
-   */
-  explicit TimedCommand(double timeout);
-
-  /**
-   * Creates a new TimedCommand with the given name and timeout.
-   *
-   * @param name      the name of the command
-   * @param timeout   the time (in seconds) before this command "times out"
-   * @param subsystem the subsystem that the command requires
-   */
-  TimedCommand(const wpi::Twine& name, double timeout, Subsystem& subsystem);
-
-  /**
-   * Creates a new WaitCommand with the given timeout.
-   *
-   * @param timeout   the time (in seconds) before this command "times out"
-   * @param subsystem the subsystem that the command requires
-   */
-  TimedCommand(double timeout, Subsystem& subsystem);
-
-  virtual ~TimedCommand() = default;
-
-  TimedCommand(TimedCommand&&) = default;
-  TimedCommand& operator=(TimedCommand&&) = default;
-
- protected:
-  /**
-   * Ends command when timed out.
-   */
-  bool IsFinished() override;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/WaitCommand.h b/wpilibc/src/main/native/include/frc/commands/WaitCommand.h
deleted file mode 100644
index 481b1c6..0000000
--- a/wpilibc/src/main/native/include/frc/commands/WaitCommand.h
+++ /dev/null
@@ -1,39 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/TimedCommand.h"
-
-namespace frc {
-
-class WaitCommand : public TimedCommand {
- public:
-  /**
-   * Creates a new WaitCommand with the given name and timeout.
-   *
-   * @param name    the name of the command
-   * @param timeout the time (in seconds) before this command "times out"
-   */
-  explicit WaitCommand(double timeout);
-
-  /**
-   * Creates a new WaitCommand with the given timeout.
-   *
-   * @param timeout the time (in seconds) before this command "times out"
-   */
-  WaitCommand(const wpi::Twine& name, double timeout);
-
-  virtual ~WaitCommand() = default;
-
-  WaitCommand(WaitCommand&&) = default;
-  WaitCommand& operator=(WaitCommand&&) = default;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/WaitForChildren.h b/wpilibc/src/main/native/include/frc/commands/WaitForChildren.h
deleted file mode 100644
index 6f2e285..0000000
--- a/wpilibc/src/main/native/include/frc/commands/WaitForChildren.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-class WaitForChildren : public Command {
- public:
-  explicit WaitForChildren(double timeout);
-  WaitForChildren(const wpi::Twine& name, double timeout);
-  virtual ~WaitForChildren() = default;
-
-  WaitForChildren(WaitForChildren&&) = default;
-  WaitForChildren& operator=(WaitForChildren&&) = default;
-
- protected:
-  virtual bool IsFinished();
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/commands/WaitUntilCommand.h b/wpilibc/src/main/native/include/frc/commands/WaitUntilCommand.h
deleted file mode 100644
index 1231750..0000000
--- a/wpilibc/src/main/native/include/frc/commands/WaitUntilCommand.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-class WaitUntilCommand : public Command {
- public:
-  /**
-   * A WaitCommand will wait until a certain match time before finishing.
-   *
-   * This will wait until the game clock reaches some value, then continue to
-   * the next command.
-   *
-   * @see CommandGroup
-   */
-  explicit WaitUntilCommand(double time);
-
-  WaitUntilCommand(const wpi::Twine& name, double time);
-
-  virtual ~WaitUntilCommand() = default;
-
-  WaitUntilCommand(WaitUntilCommand&&) = default;
-  WaitUntilCommand& operator=(WaitUntilCommand&&) = default;
-
- protected:
-  /**
-   * Check if we've reached the actual finish time.
-   */
-  virtual bool IsFinished();
-
- private:
-  double m_time;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
new file mode 100644
index 0000000..6f6e086
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
@@ -0,0 +1,149 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/MathExtras.h>
+
+namespace frc {
+/**
+ * A helper class that computes feedforward outputs for a simple arm (modeled as
+ * a motor acting against the force of gravity on a beam suspended at an angle).
+ */
+class ArmFeedforward {
+  using Angle = units::radians;
+  using Velocity = units::radians_per_second;
+  using Acceleration = units::compound_unit<units::radians_per_second,
+                                            units::inverse<units::second>>;
+  using kv_unit =
+      units::compound_unit<units::volts,
+                           units::inverse<units::radians_per_second>>;
+  using ka_unit =
+      units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+ public:
+  constexpr ArmFeedforward() = default;
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains.
+   *
+   * @param kS   The static gain, in volts.
+   * @param kCos The gravity gain, in volts.
+   * @param kV   The velocity gain, in volt seconds per radian.
+   * @param kA   The acceleration gain, in volt seconds^2 per radian.
+   */
+  constexpr ArmFeedforward(
+      units::volt_t kS, units::volt_t kCos, units::unit_t<kv_unit> kV,
+      units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
+      : kS(kS), kCos(kCos), kV(kV), kA(kA) {}
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param angle        The angle setpoint, in radians.
+   * @param velocity     The velocity setpoint, in radians per second.
+   * @param acceleration The acceleration setpoint, in radians per second^2.
+   * @return The computed feedforward, in volts.
+   */
+  units::volt_t Calculate(units::unit_t<Angle> angle,
+                          units::unit_t<Velocity> velocity,
+                          units::unit_t<Acceleration> acceleration =
+                              units::unit_t<Acceleration>(0)) const {
+    return kS * wpi::sgn(velocity) + kCos * units::math::cos(angle) +
+           kV * velocity + kA * acceleration;
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply,
+   * a position, and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm
+   * @param acceleration The acceleration of the arm.
+   * @return The maximum possible velocity at the given acceleration and angle.
+   */
+  units::unit_t<Velocity> MaxAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Angle> angle,
+      units::unit_t<Acceleration> acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - kS - kCos * units::math::cos(angle) -
+            kA * acceleration) /
+           kV;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply,
+   * a position, and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm
+   * @param acceleration The acceleration of the arm.
+   * @return The minimum possible velocity at the given acceleration and angle.
+   */
+  units::unit_t<Velocity> MinAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Angle> angle,
+      units::unit_t<Acceleration> acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + kS - kCos * units::math::cos(angle) -
+            kA * acceleration) /
+           kV;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply, a position, and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm
+   * @param velocity The velocity of the arm.
+   * @return The maximum possible acceleration at the given velocity and angle.
+   */
+  units::unit_t<Acceleration> MaxAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Angle> angle,
+      units::unit_t<Velocity> velocity) {
+    return (maxVoltage - kS * wpi::sgn(velocity) -
+            kCos * units::math::cos(angle) - kV * velocity) /
+           kA;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage
+   * supply, a position, and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the arm.
+   * @param angle The angle of the arm
+   * @param velocity The velocity of the arm.
+   * @return The minimum possible acceleration at the given velocity and angle.
+   */
+  units::unit_t<Acceleration> MinAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Angle> angle,
+      units::unit_t<Velocity> velocity) {
+    return MaxAchievableAcceleration(-maxVoltage, angle, velocity);
+  }
+
+  units::volt_t kS{0};
+  units::volt_t kCos{0};
+  units::unit_t<kv_unit> kV{0};
+  units::unit_t<ka_unit> kA{0};
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h
new file mode 100644
index 0000000..c664fc4
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -0,0 +1,131 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/MathExtras.h>
+
+namespace frc {
+/**
+ * A helper class that computes feedforward outputs for a simple elevator
+ * (modeled as a motor acting against the force of gravity).
+ */
+template <class Distance>
+class ElevatorFeedforward {
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
+  using ka_unit =
+      units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+ public:
+  ElevatorFeedforward() = default;
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains.
+   *
+   * @param kS The static gain, in volts.
+   * @param kG The gravity gain, in volts.
+   * @param kV The velocity gain, in volt seconds per distance.
+   * @param kA The acceleration gain, in volt seconds^2 per distance.
+   */
+  constexpr ElevatorFeedforward(
+      units::volt_t kS, units::volt_t kG, units::unit_t<kv_unit> kV,
+      units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
+      : kS(kS), kG(kG), kV(kV), kA(kA) {}
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity     The velocity setpoint, in distance per second.
+   * @param acceleration The acceleration setpoint, in distance per second^2.
+   * @return The computed feedforward, in volts.
+   */
+  constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
+                                    units::unit_t<Acceleration> acceleration =
+                                        units::unit_t<Acceleration>(0)) {
+    return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MaxAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - kS - kG - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param acceleration The acceleration of the elevator.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MinAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + kS - kG - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+    return (maxVoltage - kS * wpi::sgn(velocity) - kG - kV * velocity) / kA;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the elevator.
+   * @param velocity The velocity of the elevator.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+    return MaxAchievableAcceleration(-maxVoltage, velocity);
+  }
+
+  units::volt_t kS{0};
+  units::volt_t kG{0};
+  units::unit_t<kv_unit> kV{0};
+  units::unit_t<ka_unit> kA{0};
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
index 64da9eb..96fd331 100644
--- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <algorithm>
+#include <cmath>
 #include <functional>
 #include <limits>
 
@@ -14,6 +16,7 @@
 
 #include "frc/controller/PIDController.h"
 #include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableHelper.h"
 #include "frc/trajectory/TrapezoidProfile.h"
 
@@ -23,8 +26,20 @@
  * Implements a PID control loop whose setpoint is constrained by a trapezoid
  * profile.
  */
-class ProfiledPIDController : public Sendable,
-                              public SendableHelper<ProfiledPIDController> {
+template <class Distance>
+class ProfiledPIDController
+    : public Sendable,
+      public SendableHelper<ProfiledPIDController<Distance>> {
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using Acceleration_t = units::unit_t<Acceleration>;
+  using State = typename TrapezoidProfile<Distance>::State;
+  using Constraints = typename TrapezoidProfile<Distance>::Constraints;
+
  public:
   /**
    * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
@@ -38,8 +53,8 @@
    *                    default is 20 milliseconds.
    */
   ProfiledPIDController(double Kp, double Ki, double Kd,
-                        frc::TrapezoidProfile::Constraints constraints,
-                        units::second_t period = 20_ms);
+                        Constraints constraints, units::second_t period = 20_ms)
+      : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {}
 
   ~ProfiledPIDController() override = default;
 
@@ -57,96 +72,98 @@
    * @param Ki Integral coefficient
    * @param Kd Differential coefficient
    */
-  void SetPID(double Kp, double Ki, double Kd);
+  void SetPID(double Kp, double Ki, double Kd) {
+    m_controller.SetPID(Kp, Ki, Kd);
+  }
 
   /**
    * Sets the proportional coefficient of the PID controller gain.
    *
    * @param Kp proportional coefficient
    */
-  void SetP(double Kp);
+  void SetP(double Kp) { m_controller.SetP(Kp); }
 
   /**
    * Sets the integral coefficient of the PID controller gain.
    *
    * @param Ki integral coefficient
    */
-  void SetI(double Ki);
+  void SetI(double Ki) { m_controller.SetI(Ki); }
 
   /**
    * Sets the differential coefficient of the PID controller gain.
    *
    * @param Kd differential coefficient
    */
-  void SetD(double Kd);
+  void SetD(double Kd) { m_controller.SetD(Kd); }
 
   /**
    * Gets the proportional coefficient.
    *
    * @return proportional coefficient
    */
-  double GetP() const;
+  double GetP() const { return m_controller.GetP(); }
 
   /**
    * Gets the integral coefficient.
    *
    * @return integral coefficient
    */
-  double GetI() const;
+  double GetI() const { return m_controller.GetI(); }
 
   /**
    * Gets the differential coefficient.
    *
    * @return differential coefficient
    */
-  double GetD() const;
+  double GetD() const { return m_controller.GetD(); }
 
   /**
    * Gets the period of this controller.
    *
    * @return The period of the controller.
    */
-  units::second_t GetPeriod() const;
+  units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
 
   /**
    * Sets the goal for the ProfiledPIDController.
    *
    * @param goal The desired unprofiled setpoint.
    */
-  void SetGoal(TrapezoidProfile::State goal);
+  void SetGoal(State goal) { m_goal = goal; }
 
   /**
    * Sets the goal for the ProfiledPIDController.
    *
    * @param goal The desired unprofiled setpoint.
    */
-  void SetGoal(units::meter_t goal);
+  void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
 
   /**
    * Gets the goal for the ProfiledPIDController.
    */
-  TrapezoidProfile::State GetGoal() const;
+  State GetGoal() const { return m_goal; }
 
   /**
    * Returns true if the error is within the tolerance of the error.
    *
    * This will return false until at least one input value has been computed.
    */
-  bool AtGoal() const;
+  bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
 
   /**
    * Set velocity and acceleration constraints for goal.
    *
    * @param constraints Velocity and acceleration constraints for goal.
    */
-  void SetConstraints(frc::TrapezoidProfile::Constraints constraints);
+  void SetConstraints(Constraints constraints) { m_constraints = constraints; }
 
   /**
    * Returns the current setpoint of the ProfiledPIDController.
    *
    * @return The current setpoint.
    */
-  TrapezoidProfile::State GetSetpoint() const;
+  State GetSetpoint() const { return m_setpoint; }
 
   /**
    * Returns true if the error is within the tolerance of the error.
@@ -157,7 +174,7 @@
    *
    * This will return false until at least one input value has been computed.
    */
-  bool AtSetpoint() const;
+  bool AtSetpoint() const { return m_controller.AtSetpoint(); }
 
   /**
    * Enables continuous input.
@@ -169,12 +186,15 @@
    * @param minimumInput The minimum value expected from the input.
    * @param maximumInput The maximum value expected from the input.
    */
-  void EnableContinuousInput(double minimumInput, double maximumInput);
+  void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
+    m_controller.EnableContinuousInput(minimumInput.template to<double>(),
+                                       maximumInput.template to<double>());
+  }
 
   /**
    * Disables continuous input.
    */
-  void DisableContinuousInput();
+  void DisableContinuousInput() { m_controller.DisableContinuousInput(); }
 
   /**
    * Sets the minimum and maximum values for the integrator.
@@ -185,7 +205,9 @@
    * @param minimumIntegral The minimum value of the integrator.
    * @param maximumIntegral The maximum value of the integrator.
    */
-  void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
+  void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
+    m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
+  }
 
   /**
    * Sets the error which is considered tolerable for use with
@@ -195,27 +217,39 @@
    * @param velocityTolerance Velocity error which is tolerable.
    */
   void SetTolerance(
-      double positionTolerance,
-      double velocityTolerance = std::numeric_limits<double>::infinity());
+      Distance_t positionTolerance,
+      Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
+    m_controller.SetTolerance(positionTolerance.template to<double>(),
+                              velocityTolerance.template to<double>());
+  }
 
   /**
    * Returns the difference between the setpoint and the measurement.
    *
    * @return The error.
    */
-  double GetPositionError() const;
+  Distance_t GetPositionError() const {
+    return Distance_t(m_controller.GetPositionError());
+  }
 
   /**
    * Returns the change in error per second.
    */
-  double GetVelocityError() const;
+  Velocity_t GetVelocityError() const {
+    return Velocity_t(m_controller.GetVelocityError());
+  }
 
   /**
    * Returns the next output of the PID controller.
    *
    * @param measurement The current measurement of the process variable.
    */
-  double Calculate(units::meter_t measurement);
+  double Calculate(Distance_t measurement) {
+    frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
+    m_setpoint = profile.Calculate(GetPeriod());
+    return m_controller.Calculate(measurement.template to<double>(),
+                                  m_setpoint.position.template to<double>());
+  }
 
   /**
    * Returns the next output of the PID controller.
@@ -223,15 +257,20 @@
    * @param measurement The current measurement of the process variable.
    * @param goal The new goal of the controller.
    */
-  double Calculate(units::meter_t measurement, TrapezoidProfile::State goal);
-
+  double Calculate(Distance_t measurement, State goal) {
+    SetGoal(goal);
+    return Calculate(measurement);
+  }
   /**
    * Returns the next output of the PID controller.
    *
    * @param measurement The current measurement of the process variable.
    * @param goal The new goal of the controller.
    */
-  double Calculate(units::meter_t measurement, units::meter_t goal);
+  double Calculate(Distance_t measurement, Distance_t goal) {
+    SetGoal(goal);
+    return Calculate(measurement);
+  }
 
   /**
    * Returns the next output of the PID controller.
@@ -240,21 +279,36 @@
    * @param goal        The new goal of the controller.
    * @param constraints Velocity and acceleration constraints for goal.
    */
-  double Calculate(units::meter_t measurement, units::meter_t goal,
-                   frc::TrapezoidProfile::Constraints constraints);
+  double Calculate(
+      Distance_t measurement, Distance_t goal,
+      typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
+    SetConstraints(constraints);
+    return Calculate(measurement, goal);
+  }
 
   /**
    * Reset the previous error, the integral term, and disable the controller.
    */
-  void Reset();
+  void Reset() { m_controller.Reset(); }
 
-  void InitSendable(frc::SendableBuilder& builder) override;
+  void InitSendable(frc::SendableBuilder& builder) override {
+    builder.SetSmartDashboardType("ProfiledPIDController");
+    builder.AddDoubleProperty("p", [this] { return GetP(); },
+                              [this](double value) { SetP(value); });
+    builder.AddDoubleProperty("i", [this] { return GetI(); },
+                              [this](double value) { SetI(value); });
+    builder.AddDoubleProperty("d", [this] { return GetD(); },
+                              [this](double value) { SetD(value); });
+    builder.AddDoubleProperty(
+        "goal", [this] { return GetGoal().position.template to<double>(); },
+        [this](double value) { SetGoal(Distance_t{value}); });
+  }
 
  private:
   frc2::PIDController m_controller;
-  frc::TrapezoidProfile::State m_goal;
-  frc::TrapezoidProfile::State m_setpoint;
-  frc::TrapezoidProfile::Constraints m_constraints;
+  typename frc::TrapezoidProfile<Distance>::State m_goal;
+  typename frc::TrapezoidProfile<Distance>::State m_setpoint;
+  typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/RamseteController.h b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
index 3a564aa..6ec6edc 100644
--- a/wpilibc/src/main/native/include/frc/controller/RamseteController.h
+++ b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
@@ -55,6 +55,13 @@
   RamseteController(double b, double zeta);
 
   /**
+   * Construct a Ramsete unicycle controller. The default arguments for
+   * b and zeta of 2.0 and 0.7 have been well-tested to produce desireable
+   * results.
+   */
+  RamseteController() : RamseteController(2.0, 0.7) {}
+
+  /**
    * Returns true if the pose error is within tolerance of the reference.
    */
   bool AtReference() const;
diff --git a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
new file mode 100644
index 0000000..8f82cb9
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+#include <wpi/MathExtras.h>
+
+namespace frc {
+/**
+ * A helper class that computes feedforward voltages for a simple
+ * permanent-magnet DC motor.
+ */
+template <class Distance>
+class SimpleMotorFeedforward {
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using kv_unit = units::compound_unit<units::volts, units::inverse<Velocity>>;
+  using ka_unit =
+      units::compound_unit<units::volts, units::inverse<Acceleration>>;
+
+ public:
+  constexpr SimpleMotorFeedforward() = default;
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains.
+   *
+   * @param kS The static gain, in volts.
+   * @param kV The velocity gain, in volt seconds per distance.
+   * @param kA The acceleration gain, in volt seconds^2 per distance.
+   */
+  constexpr SimpleMotorFeedforward(
+      units::volt_t kS, units::unit_t<kv_unit> kV,
+      units::unit_t<ka_unit> kA = units::unit_t<ka_unit>(0))
+      : kS(kS), kV(kV), kA(kA) {}
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity     The velocity setpoint, in distance per second.
+   * @param acceleration The acceleration setpoint, in distance per second^2.
+   * @return The computed feedforward, in volts.
+   */
+  constexpr units::volt_t Calculate(units::unit_t<Velocity> velocity,
+                                    units::unit_t<Acceleration> acceleration =
+                                        units::unit_t<Acceleration>(0)) const {
+    return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the maximum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The maximum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MaxAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - kS - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the minimum achievable velocity given a maximum voltage supply
+   * and an acceleration.  Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the acceleration constraint, and this will give you
+   * a simultaneously-achievable velocity constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param acceleration The acceleration of the motor.
+   * @return The minimum possible velocity at the given acceleration.
+   */
+  constexpr units::unit_t<Velocity> MinAchievableVelocity(
+      units::volt_t maxVoltage, units::unit_t<Acceleration> acceleration) {
+    // Assume min velocity is positive, ks flips sign
+    return (-maxVoltage + kS - kA * acceleration) / kV;
+  }
+
+  /**
+   * Calculates the maximum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The maximum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MaxAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+    return (maxVoltage - kS * wpi::sgn(velocity) - kV * velocity) / kA;
+  }
+
+  /**
+   * Calculates the minimum achievable acceleration given a maximum voltage
+   * supply and a velocity. Useful for ensuring that velocity and
+   * acceleration constraints for a trapezoidal profile are simultaneously
+   * achievable - enter the velocity constraint, and this will give you
+   * a simultaneously-achievable acceleration constraint.
+   *
+   * @param maxVoltage The maximum voltage that can be supplied to the motor.
+   * @param velocity The velocity of the motor.
+   * @return The minimum possible acceleration at the given velocity.
+   */
+  constexpr units::unit_t<Acceleration> MinAchievableAcceleration(
+      units::volt_t maxVoltage, units::unit_t<Velocity> velocity) {
+    return MaxAchievableAcceleration(-maxVoltage, velocity);
+  }
+
+  units::volt_t kS{0};
+  units::unit_t<kv_unit> kV{0};
+  units::unit_t<ka_unit> kA{0};
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
index 192d21b..c6b705a 100644
--- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
@@ -30,12 +30,12 @@
  * @code{.cpp}
  * class Robot {
  *  public:
- *   frc::Spark m_frontLeft{1};
- *   frc::Spark m_rearLeft{2};
+ *   frc::PWMVictorSPX m_frontLeft{1};
+ *   frc::PWMVictorSPX m_rearLeft{2};
  *   frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
  *
- *   frc::Spark m_frontRight{3};
- *   frc::Spark m_rearRight{4};
+ *   frc::PWMVictorSPX m_frontRight{3};
+ *   frc::PWMVictorSPX m_rearRight{4};
  *   frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight};
  *
  *   frc::DifferentialDrive m_drive{m_left, m_right};
@@ -46,14 +46,14 @@
  * @code{.cpp}
  * class Robot {
  *  public:
- *   frc::Spark m_frontLeft{1};
- *   frc::Spark m_midLeft{2};
- *   frc::Spark m_rearLeft{3};
+ *   frc::PWMVictorSPX m_frontLeft{1};
+ *   frc::PWMVictorSPX m_midLeft{2};
+ *   frc::PWMVictorSPX m_rearLeft{3};
  *   frc::SpeedControllerGroup m_left{m_frontLeft, m_midLeft, m_rearLeft};
  *
- *   frc::Spark m_frontRight{4};
- *   frc::Spark m_midRight{5};
- *   frc::Spark m_rearRight{6};
+ *   frc::PWMVictorSPX m_frontRight{4};
+ *   frc::PWMVictorSPX m_midRight{5};
+ *   frc::PWMVictorSPX m_rearRight{6};
  *   frc::SpeedControllerGroup m_right{m_frontRight, m_midRight, m_rearRight};
  *
  *   frc::DifferentialDrive m_drive{m_left, m_right};
diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
index 2161e82..46328a0 100644
--- a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
+++ b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
@@ -11,6 +11,10 @@
 #include "Translation2d.h"
 #include "Twist2d.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -146,8 +150,8 @@
    *
    * @param twist The change in pose in the robot's coordinate frame since the
    * previous pose update. For example, if a non-holonomic robot moves forward
-   * 0.01 meters and changes angle by .5 degrees since the previous pose update,
-   * the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+   * 0.01 meters and changes angle by 0.5 degrees since the previous pose
+   * update, the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
    *
    * @return The new pose of the robot.
    */
@@ -167,4 +171,9 @@
   Translation2d m_translation;
   Rotation2d m_rotation;
 };
+
+void to_json(wpi::json& json, const Pose2d& pose);
+
+void from_json(const wpi::json& json, Pose2d& pose);
+
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
index 9453e0c..b636513 100644
--- a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
@@ -8,7 +8,10 @@
 #pragma once
 
 #include <units/units.h>
-#include <wpi/math>
+
+namespace wpi {
+class json;
+}  // namespace wpi
 
 namespace frc {
 
@@ -175,4 +178,9 @@
   double m_cos = 1;
   double m_sin = 0;
 };
+
+void to_json(wpi::json& json, const Rotation2d& rotation);
+
+void from_json(const wpi::json& json, Rotation2d& rotation);
+
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
index a53abf1..90c61e2 100644
--- a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
@@ -11,6 +11,10 @@
 
 #include "Rotation2d.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -211,4 +215,9 @@
   units::meter_t m_x = 0_m;
   units::meter_t m_y = 0_m;
 };
+
+void to_json(wpi::json& json, const Translation2d& state);
+
+void from_json(const wpi::json& json, Translation2d& state);
+
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index 34407b9..cb2121f 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -31,7 +31,8 @@
    * empirical value may be larger than the physical measured value due to
    * scrubbing effects.
    */
-  explicit DifferentialDriveKinematics(units::meter_t trackWidth);
+  constexpr explicit DifferentialDriveKinematics(units::meter_t trackWidth)
+      : trackWidth(trackWidth) {}
 
   /**
    * Returns a chassis speed from left and right component velocities using
@@ -40,8 +41,11 @@
    * @param wheelSpeeds The left and right velocities.
    * @return The chassis speed.
    */
-  ChassisSpeeds ToChassisSpeeds(
-      const DifferentialDriveWheelSpeeds& wheelSpeeds) const;
+  constexpr ChassisSpeeds ToChassisSpeeds(
+      const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
+    return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
+            (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
+  }
 
   /**
    * Returns left and right component velocities from a chassis speed using
@@ -51,10 +55,12 @@
    * represent the chassis' speed.
    * @return The left and right velocities.
    */
-  DifferentialDriveWheelSpeeds ToWheelSpeeds(
-      const ChassisSpeeds& chassisSpeeds) const;
+  constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const {
+    return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
+            chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
+  }
 
- private:
-  units::meter_t m_trackWidth;
+  units::meter_t trackWidth;
 };
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
index fd41d15..379839d 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -23,29 +23,38 @@
  * path following. Furthermore, odometry can be used for latency compensation
  * when using computer-vision systems.
  *
- * Note: It is important to reset both your encoders to zero before you start
- * using this class. Only reset your encoders ONCE. You should not reset your
- * encoders even if you want to reset your robot's pose.
+ * It is important that you reset your encoders to zero before using this class.
+ * Any subsequent pose resets also require the encoders to be reset to zero.
  */
 class DifferentialDriveOdometry {
  public:
   /**
    * Constructs a DifferentialDriveOdometry object.
    *
-   * @param kinematics The differential drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
    * @param initialPose The starting position of the robot on the field.
    */
-  explicit DifferentialDriveOdometry(DifferentialDriveKinematics kinematics,
+  explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
                                      const Pose2d& initialPose = Pose2d());
 
   /**
    * Resets the robot's position on the field.
    *
+   * You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
    * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
    */
-  void ResetPosition(const Pose2d& pose) {
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
     m_pose = pose;
     m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+
+    m_prevLeftDistance = 0_m;
+    m_prevRightDistance = 0_m;
   }
 
   /**
@@ -55,46 +64,26 @@
   const Pose2d& GetPose() const { return m_pose; }
 
   /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method takes in the current time as
-   * a parameter to calculate period (difference between two timestamps). The
-   * period is used to calculate the change in distance from a velocity. This
-   * also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
+   * Updates the robot position on the field using distance measurements from
+   * encoders. This method is more numerically accurate than using velocities to
+   * integrate the pose and is also advantageous for teams that are using lower
+   * CPR encoders.
    *
-   * @param currentTime The current time.
-   * @param angle The angle of the robot.
-   * @param wheelSpeeds The current wheel speeds.
-   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param leftDistance The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
    * @return The new pose of the robot.
    */
-  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& angle,
-                               const DifferentialDriveWheelSpeeds& wheelSpeeds);
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method automatically calculates
-   * the current time to calculate period (difference between two timestamps).
-   * The period is used to calculate the change in distance from a velocity.
-   * This also takes in an angle parameter which is used instead of the
-   * angular rate that is calculated from forward kinematics.
-   *
-   * @param angle The angle of the robot.
-   * @param wheelSpeeds     The current wheel speeds.
-   *
-   * @return The new pose of the robot.
-   */
-  const Pose2d& Update(const Rotation2d& angle,
-                       const DifferentialDriveWheelSpeeds& wheelSpeeds) {
-    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
-  }
+  const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
+                       units::meter_t rightDistance);
 
  private:
-  DifferentialDriveKinematics m_kinematics;
   Pose2d m_pose;
 
-  units::second_t m_previousTime = -1_s;
+  Rotation2d m_gyroOffset;
   Rotation2d m_previousAngle;
+
+  units::meter_t m_prevLeftDistance = 0_m;
+  units::meter_t m_prevRightDistance = 0_m;
 };
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
index e8816e1..697a6b0 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -31,19 +31,26 @@
    * Constructs a MecanumDriveOdometry object.
    *
    * @param kinematics The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
    * @param initialPose The starting position of the robot on the field.
    */
   explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
+                                const Rotation2d& gyroAngle,
                                 const Pose2d& initialPose = Pose2d());
 
   /**
    * Resets the robot's position on the field.
    *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
    * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
    */
-  void ResetPosition(const Pose2d& pose) {
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
     m_pose = pose;
     m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
   }
 
   /**
@@ -61,13 +68,13 @@
    * angular rate that is calculated from forward kinematics.
    *
    * @param currentTime The current time.
-   * @param angle The angle of the robot.
+   * @param gyroAngle The angle reported by the gyroscope.
    * @param wheelSpeeds The current wheel speeds.
    *
    * @return The new pose of the robot.
    */
   const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& angle,
+                               const Rotation2d& gyroAngle,
                                MecanumDriveWheelSpeeds wheelSpeeds);
 
   /**
@@ -78,14 +85,15 @@
    * This also takes in an angle parameter which is used instead of the
    * angular rate that is calculated from forward kinematics.
    *
-   * @param angle The angle of the robot.
-   * @param wheelSpeeds     The current wheel speeds.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelSpeeds The current wheel speeds.
    *
    * @return The new pose of the robot.
    */
-  const Pose2d& Update(const Rotation2d& angle,
+  const Pose2d& Update(const Rotation2d& gyroAngle,
                        MecanumDriveWheelSpeeds wheelSpeeds) {
-    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle, wheelSpeeds);
+    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
+                          wheelSpeeds);
   }
 
  private:
@@ -94,6 +102,7 @@
 
   units::second_t m_previousTime = -1_s;
   Rotation2d m_previousAngle;
+  Rotation2d m_gyroOffset;
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index fcd8087..1bdbcea 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -122,6 +122,22 @@
   ChassisSpeeds ToChassisSpeeds(ModuleStates&&... wheelStates);
 
   /**
+   * Performs forward kinematics to return the resulting chassis state from the
+   * given module states. This method is often used for odometry -- determining
+   * the robot's position on the field using data from the real-world speed and
+   * angle of each module on the robot.
+   *
+   * @param moduleStates The state of the modules as an std::array of type
+   * SwerveModuleState, NumModules long as measured from respective encoders
+   * and gyros. The order of the swerve module states should be same as passed
+   * into the constructor of this class.
+   *
+   * @return The resulting chassis speed.
+   */
+  ChassisSpeeds ToChassisSpeeds(
+      std::array<SwerveModuleState, NumModules> moduleStates);
+
+  /**
    * Normalizes the wheel speeds using some max attainable speed. Sometimes,
    * after inverse kinematics, the requested speed from a/several modules may be
    * above the max attainable speed for the driving motor on that module. To fix
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index 138954d..b362aa0 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -63,6 +63,13 @@
                 "locations provided in constructor.");
 
   std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
+
+  return this->ToChassisSpeeds(moduleStates);
+}
+
+template <size_t NumModules>
+ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
+    std::array<SwerveModuleState, NumModules> moduleStates) {
   Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
 
   for (size_t i = 0; i < NumModules; i++) {
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
index 1dbe178..8093ca5 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -36,19 +36,26 @@
    * Constructs a SwerveDriveOdometry object.
    *
    * @param kinematics The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
    * @param initialPose The starting position of the robot on the field.
    */
   SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
+                      const Rotation2d& gyroAngle,
                       const Pose2d& initialPose = Pose2d());
 
   /**
    * Resets the robot's position on the field.
    *
+   * The gyroscope angle does not need to be reset here on the user's robot
+   * code. The library automatically takes care of offsetting the gyro angle.
+   *
    * @param pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
    */
-  void ResetPosition(const Pose2d& pose) {
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
     m_pose = pose;
     m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
   }
 
   /**
@@ -66,7 +73,7 @@
    * angular rate that is calculated from forward kinematics.
    *
    * @param currentTime The current time.
-   * @param angle The angle of the robot.
+   * @param gyroAngle The angle reported by the gyroscope.
    * @param moduleStates The current state of all swerve modules. Please provide
    *                     the states in the same order in which you instantiated
    *                     your SwerveDriveKinematics.
@@ -75,7 +82,7 @@
    */
   template <typename... ModuleStates>
   const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& angle,
+                               const Rotation2d& gyroAngle,
                                ModuleStates&&... moduleStates);
 
   /**
@@ -86,7 +93,7 @@
    * This also takes in an angle parameter which is used instead of the
    * angular rate that is calculated from forward kinematics.
    *
-   * @param angle The angle of the robot.
+   * @param gyroAngle The angle reported by the gyroscope.
    * @param moduleStates The current state of all swerve modules. Please provide
    *                     the states in the same order in which you instantiated
    *                     your SwerveDriveKinematics.
@@ -94,9 +101,9 @@
    * @return The new pose of the robot.
    */
   template <typename... ModuleStates>
-  const Pose2d& Update(const Rotation2d& angle,
+  const Pose2d& Update(const Rotation2d& gyroAngle,
                        ModuleStates&&... moduleStates) {
-    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), angle,
+    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
                           moduleStates...);
   }
 
@@ -106,6 +113,7 @@
 
   units::second_t m_previousTime = -1_s;
   Rotation2d m_previousAngle;
+  Rotation2d m_gyroOffset;
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index 1ff75ac..23852d0 100644
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -10,20 +10,24 @@
 namespace frc {
 template <size_t NumModules>
 SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
-    SwerveDriveKinematics<NumModules> kinematics, const Pose2d& initialPose)
+    SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
+    const Pose2d& initialPose)
     : m_kinematics(kinematics), m_pose(initialPose) {
   m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
 }
 
 template <size_t NumModules>
 template <typename... ModuleStates>
 const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& angle,
+    units::second_t currentTime, const Rotation2d& gyroAngle,
     ModuleStates&&... moduleStates) {
   units::second_t deltaTime =
       (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
   m_previousTime = currentTime;
 
+  auto angle = gyroAngle + m_gyroOffset;
+
   auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
   static_cast<void>(dtheta);
 
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h b/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
index 97805ba..9fdc049 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
@@ -11,6 +11,7 @@
 #include <string>
 #include <vector>
 
+#include <networktables/NetworkTableEntry.h>
 #include <networktables/NetworkTableValue.h>
 
 #include "frc/ErrorBase.h"
@@ -97,6 +98,16 @@
   static void Delete(wpi::StringRef key);
 
   /**
+   * Returns an NT Entry mapping to the specified key
+   *
+   * This is useful if an entry is used often, or is read and then modified.
+   *
+   * @param key the key
+   * @return    the entry for the key
+   */
+  static nt::NetworkTableEntry GetEntry(wpi::StringRef key);
+
+  /**
    * Maps the specified key to the specified value in this table.
    *
    * The value can be retrieved by calling the get method with a key that is
diff --git a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
index a8979c1..1e08a94 100644
--- a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -48,7 +48,8 @@
   }
 
  private:
-  Eigen::Matrix<double, 6, 4> m_coefficients;
+  Eigen::Matrix<double, 6, 4> m_coefficients =
+      Eigen::Matrix<double, 6, 4>::Zero();
 
   /**
    * Returns the hermite basis matrix for cubic hermite spline interpolation.
diff --git a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
index 730eab4..632d3ad 100644
--- a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
+++ b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
@@ -48,7 +48,8 @@
   }
 
  private:
-  Eigen::Matrix<double, 6, 6> m_coefficients;
+  Eigen::Matrix<double, 6, 6> m_coefficients =
+      Eigen::Matrix<double, 6, 6>::Zero();
 
   /**
    * Returns the hermite basis matrix for quintic hermite spline interpolation.
diff --git a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
index 0a384fb..96c2c6e 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
@@ -13,6 +13,10 @@
 
 #include "frc/geometry/Pose2d.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -48,6 +52,22 @@
     curvature_t curvature{0.0};
 
     /**
+     * Checks equality between this State and another object.
+     *
+     * @param other The other object.
+     * @return Whether the two objects are equal.
+     */
+    bool operator==(const State& other) const;
+
+    /**
+     * Checks inequality between this State and another object.
+     *
+     * @param other The other object.
+     * @return Whether the two objects are not equal.
+     */
+    bool operator!=(const State& other) const;
+
+    /**
      * Interpolates between two States.
      *
      * @param endValue The end value for the interpolation.
@@ -103,4 +123,9 @@
     return startValue + (endValue - startValue) * t;
   }
 };
+
+void to_json(wpi::json& json, const Trajectory::State& state);
+
+void from_json(const wpi::json& json, Trajectory::State& state);
+
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
index a739070..36118a0 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
@@ -14,7 +14,11 @@
 #include <units/units.h>
 
 #include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
+#include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
 
 namespace frc {
@@ -89,6 +93,27 @@
   }
 
   /**
+   * Adds a mecanum drive kinematics constraint to ensure that
+   * no wheel velocity of a mecanum drive goes above the max velocity.
+   *
+   * @param kinematics The mecanum drive kinematics.
+   */
+  void SetKinematics(MecanumDriveKinematics kinematics) {
+    AddConstraint(MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
+  }
+
+  /**
+   * Adds a swerve drive kinematics constraint to ensure that
+   * no wheel velocity of a swerve drive goes above the max velocity.
+   *
+   * @param kinematics The swerve drive kinematics.
+   */
+  template <size_t NumModules>
+  void SetKinematics(SwerveDriveKinematics<NumModules>& kinematics) {
+    AddConstraint(SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
+  }
+
+  /**
    * Returns the starting velocity of the trajectory.
    * @return The starting velocity of the trajectory.
    */
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h
new file mode 100644
index 0000000..700ed9c
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <string>
+
+#include <wpi/StringRef.h>
+#include <wpi/Twine.h>
+
+#include "frc/trajectory/Trajectory.h"
+
+namespace frc {
+class TrajectoryUtil {
+ public:
+  TrajectoryUtil() = delete;
+
+  /**
+   * Exports a Trajectory to a PathWeaver-style JSON file.
+   *
+   * @param trajectory the trajectory to export
+   * @param path the path of the file to export to
+   *
+   * @return The interpolated state.
+   */
+  static void ToPathweaverJson(const Trajectory& trajectory,
+                               const wpi::Twine& path);
+  /**
+   * Imports a Trajectory from a PathWeaver-style JSON file.
+   *
+   * @param path The path of the json file to import from.
+   *
+   * @return The trajectory represented by the file.
+   */
+  static Trajectory FromPathweaverJson(const wpi::Twine& path);
+
+  /**
+     * Deserializes a Trajectory from PathWeaver-style JSON.
+
+     * @param json the string containing the serialized JSON
+
+     * @return the trajectory represented by the JSON
+     */
+  static std::string SerializeTrajectory(const Trajectory& trajectory);
+
+  /**
+   * Serializes a Trajectory to PathWeaver-style JSON.
+
+   * @param trajectory the trajectory to export
+
+   * @return the string containing the serialized JSON
+   */
+  static Trajectory DeserializeTrajectory(wpi::StringRef json_str);
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index c528c9e..0479cfe 100644
--- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -40,18 +40,27 @@
  * `Calculate()` and to determine when the profile has completed via
  * `IsFinished()`.
  */
+template <class Distance>
 class TrapezoidProfile {
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using Acceleration_t = units::unit_t<Acceleration>;
+
  public:
   class Constraints {
    public:
-    units::meters_per_second_t maxVelocity = 0_mps;
-    units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
+    Velocity_t maxVelocity{0};
+    Acceleration_t maxAcceleration{0};
   };
 
   class State {
    public:
-    units::meter_t position = 0_m;
-    units::meters_per_second_t velocity = 0_mps;
+    Distance_t position{0};
+    Velocity_t velocity{0};
     bool operator==(const State& rhs) const {
       return position == rhs.position && velocity == rhs.velocity;
     }
@@ -66,7 +75,7 @@
    * @param initial     The initial state (usually the current state).
    */
   TrapezoidProfile(Constraints constraints, State goal,
-                   State initial = State{0_m, 0_mps});
+                   State initial = State{Distance_t(0), Velocity_t(0)});
 
   TrapezoidProfile(const TrapezoidProfile&) = default;
   TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
@@ -86,7 +95,7 @@
    *
    * @param target The target distance.
    */
-  units::second_t TimeLeftUntil(units::meter_t target) const;
+  units::second_t TimeLeftUntil(Distance_t target) const;
 
   /**
    * Returns the total time the profile takes to reach the goal.
@@ -135,5 +144,6 @@
   units::second_t m_endFullSpeed;
   units::second_t m_endDeccel;
 };
-
 }  // namespace frc
+
+#include "TrapezoidProfile.inc"
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
similarity index 76%
rename from wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
rename to wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 6026aa5..50f232d 100644
--- a/wpilibc/src/main/native/cpp/trajectory/TrapezoidProfile.cpp
+++ b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -5,12 +5,14 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include "frc/trajectory/TrapezoidProfile.h"
+#pragma once
 
-using namespace frc;
+#include <algorithm>
 
-TrapezoidProfile::TrapezoidProfile(Constraints constraints, State goal,
-                                   State initial)
+namespace frc {
+template <class Distance>
+TrapezoidProfile<Distance>::TrapezoidProfile(Constraints constraints,
+                                             State goal, State initial)
     : m_direction{ShouldFlipAcceleration(initial, goal) ? -1 : 1},
       m_constraints(constraints),
       m_initial(Direct(initial)),
@@ -24,30 +26,30 @@
   // ended at zero velocity
   units::second_t cutoffBegin =
       m_initial.velocity / m_constraints.maxAcceleration;
-  units::meter_t cutoffDistBegin =
+  Distance_t cutoffDistBegin =
       cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
 
   units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
-  units::meter_t cutoffDistEnd =
+  Distance_t cutoffDistEnd =
       cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
 
   // Now we can calculate the parameters as if it was a full trapezoid instead
   // of a truncated one
 
-  units::meter_t fullTrapezoidDist =
+  Distance_t fullTrapezoidDist =
       cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
   units::second_t accelerationTime =
       m_constraints.maxVelocity / m_constraints.maxAcceleration;
 
-  units::meter_t fullSpeedDist =
+  Distance_t fullSpeedDist =
       fullTrapezoidDist -
       accelerationTime * accelerationTime * m_constraints.maxAcceleration;
 
   // Handle the case where the profile never reaches full speed
-  if (fullSpeedDist < 0_m) {
+  if (fullSpeedDist < Distance_t(0)) {
     accelerationTime =
         units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
-    fullSpeedDist = 0_m;
+    fullSpeedDist = Distance_t(0);
   }
 
   m_endAccel = accelerationTime - cutoffBegin;
@@ -55,7 +57,9 @@
   m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
 }
 
-TrapezoidProfile::State TrapezoidProfile::Calculate(units::second_t t) const {
+template <class Distance>
+typename TrapezoidProfile<Distance>::State
+TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
   State result = m_initial;
 
   if (t < m_endAccel) {
@@ -83,9 +87,11 @@
   return Direct(result);
 }
 
-units::second_t TrapezoidProfile::TimeLeftUntil(units::meter_t target) const {
-  units::meter_t position = m_initial.position * m_direction;
-  units::meters_per_second_t velocity = m_initial.velocity * m_direction;
+template <class Distance>
+units::second_t TrapezoidProfile<Distance>::TimeLeftUntil(
+    Distance_t target) const {
+  Distance_t position = m_initial.position * m_direction;
+  Velocity_t velocity = m_initial.velocity * m_direction;
 
   units::second_t endAccel = m_endAccel * m_direction;
   units::second_t endFullSpeed = m_endFullSpeed * m_direction - endAccel;
@@ -101,21 +107,19 @@
   units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
   endDeccel = units::math::max(endDeccel, 0_s);
 
-  const units::meters_per_second_squared_t acceleration =
-      m_constraints.maxAcceleration;
-  const units::meters_per_second_squared_t decceleration =
-      -m_constraints.maxAcceleration;
+  const Acceleration_t acceleration = m_constraints.maxAcceleration;
+  const Acceleration_t decceleration = -m_constraints.maxAcceleration;
 
-  units::meter_t distToTarget = units::math::abs(target - position);
+  Distance_t distToTarget = units::math::abs(target - position);
 
-  if (distToTarget < 1e-6_m) {
+  if (distToTarget < Distance_t(1e-6)) {
     return 0_s;
   }
 
-  units::meter_t accelDist =
+  Distance_t accelDist =
       velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
 
-  units::meters_per_second_t deccelVelocity;
+  Velocity_t deccelVelocity;
   if (endAccel > 0_s) {
     deccelVelocity = units::math::sqrt(
         units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
@@ -123,20 +127,20 @@
     deccelVelocity = velocity;
   }
 
-  units::meter_t deccelDist =
+  Distance_t deccelDist =
       deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
 
-  deccelDist = units::math::max(deccelDist, 0_m);
+  deccelDist = units::math::max(deccelDist, Distance_t(0));
 
-  units::meter_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+  Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
 
   if (accelDist > distToTarget) {
     accelDist = distToTarget;
-    fullSpeedDist = 0_m;
-    deccelDist = 0_m;
+    fullSpeedDist = Distance_t(0);
+    deccelDist = Distance_t(0);
   } else if (accelDist + fullSpeedDist > distToTarget) {
     fullSpeedDist = distToTarget - accelDist;
-    deccelDist = 0_m;
+    deccelDist = Distance_t(0);
   } else {
     deccelDist = distToTarget - fullSpeedDist - accelDist;
   }
@@ -156,3 +160,4 @@
 
   return accelTime + fullSpeedTime + deccelTime;
 }
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
new file mode 100644
index 0000000..50ace85
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * A class that enforces constraints on differential drive voltage expenditure
+ * based on the motor dynamics and the drive kinematics.  Ensures that the
+ * acceleration of any wheel of the robot while following the trajectory is
+ * never higher than what can be achieved with the given maximum voltage.
+ */
+class DifferentialDriveVoltageConstraint : public TrajectoryConstraint {
+ public:
+  /**
+   * Creates a new DifferentialDriveVoltageConstraint.
+   *
+   * @param feedforward A feedforward component describing the behavior of the
+   * drive.
+   * @param kinematics  A kinematics component describing the drive geometry.
+   * @param maxVoltage  The maximum voltage available to the motors while
+   * following the path. Should be somewhat less than the nominal battery
+   * voltage (12V) to account for "voltage sag" due to current draw.
+   */
+  DifferentialDriveVoltageConstraint(
+      SimpleMotorFeedforward<units::meter> feedforward,
+      DifferentialDriveKinematics kinematics, units::volt_t maxVoltage);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, curvature_t curvature,
+      units::meters_per_second_t velocity) override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+                            units::meters_per_second_t speed) override;
+
+ private:
+  SimpleMotorFeedforward<units::meter> m_feedforward;
+  DifferentialDriveKinematics m_kinematics;
+  units::volt_t m_maxVoltage;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
new file mode 100644
index 0000000..53e3953
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+
+#include <units/units.h>
+
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+namespace frc {
+class MecanumDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+  MecanumDriveKinematicsConstraint(MecanumDriveKinematics kinematics,
+                                   units::meters_per_second_t maxSpeed);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, curvature_t curvature,
+      units::meters_per_second_t velocity) override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+                            units::meters_per_second_t speed) override;
+
+ private:
+  MecanumDriveKinematics m_kinematics;
+  units::meters_per_second_t m_maxSpeed;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
new file mode 100644
index 0000000..a8ad870
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <cmath>
+
+#include <units/units.h>
+
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+namespace frc {
+
+template <size_t NumModules>
+class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+  SwerveDriveKinematicsConstraint(
+      frc::SwerveDriveKinematics<NumModules> kinematics,
+      units::meters_per_second_t maxSpeed);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, curvature_t curvature,
+      units::meters_per_second_t velocity) override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+                            units::meters_per_second_t speed) override;
+
+ private:
+  frc::SwerveDriveKinematics<NumModules> m_kinematics;
+  units::meters_per_second_t m_maxSpeed;
+};
+}  // namespace frc
+
+#include "SwerveDriveKinematicsConstraint.inc"
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
new file mode 100644
index 0000000..c3437d5
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+
+namespace frc {
+
+template <size_t NumModules>
+SwerveDriveKinematicsConstraint<NumModules>::SwerveDriveKinematicsConstraint(
+    frc::SwerveDriveKinematics<NumModules> kinematics,
+    units::meters_per_second_t maxSpeed)
+    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+template <size_t NumModules>
+units::meters_per_second_t
+SwerveDriveKinematicsConstraint<NumModules>::MaxVelocity(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t velocity) {
+  auto xVelocity = velocity * pose.Rotation().Cos();
+  auto yVelocity = velocity * pose.Rotation().Sin();
+  auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
+      {xVelocity, yVelocity, velocity * curvature});
+  m_kinematics.NormalizeWheelSpeeds(&wheelSpeeds, m_maxSpeed);
+
+  auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+}
+
+template <size_t NumModules>
+TrajectoryConstraint::MinMax
+SwerveDriveKinematicsConstraint<NumModules>::MinMaxAcceleration(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t speed) {
+  return {};
+}
+
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc2/command/Command.h b/wpilibc/src/main/native/include/frc2/command/Command.h
deleted file mode 100644
index 49e904e..0000000
--- a/wpilibc/src/main/native/include/frc2/command/Command.h
+++ /dev/null
@@ -1,242 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/ErrorBase.h>
-#include <frc/WPIErrors.h>
-#include <frc2/command/Subsystem.h>
-
-#include <memory>
-#include <string>
-
-#include <units/units.h>
-#include <wpi/ArrayRef.h>
-#include <wpi/Demangle.h>
-#include <wpi/SmallSet.h>
-#include <wpi/Twine.h>
-
-namespace frc2 {
-
-template <typename T>
-std::string GetTypeName(const T& type) {
-  return wpi::Demangle(typeid(type).name());
-}
-
-class ParallelCommandGroup;
-class ParallelRaceGroup;
-class ParallelDeadlineGroup;
-class SequentialCommandGroup;
-class PerpetualCommand;
-class ProxyScheduleCommand;
-
-/**
- * A state machine representing a complete action to be performed by the robot.
- * Commands are run by the CommandScheduler, and can be composed into
- * CommandGroups to allow users to build complicated multi-step actions without
- * the need to roll the state machine logic themselves.
- *
- * <p>Commands are run synchronously from the main robot loop; no multithreading
- * is used, unless specified explicitly from the command implementation.
- *
- * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
- * or decorators will not function!
- *
- * @see CommandScheduler
- * @see CommandHelper
- */
-class Command : public frc::ErrorBase {
- public:
-  Command() = default;
-  virtual ~Command();
-
-  Command(const Command&);
-  Command& operator=(const Command&);
-  Command(Command&&) = default;
-  Command& operator=(Command&&) = default;
-
-  /**
-   * The initial subroutine of a command.  Called once when the command is
-   * initially scheduled.
-   */
-  virtual void Initialize();
-
-  /**
-   * The main body of a command.  Called repeatedly while the command is
-   * scheduled.
-   */
-  virtual void Execute();
-
-  /**
-   * The action to take when the command ends.  Called when either the command
-   * finishes normally, or when it interrupted/canceled.
-   *
-   * @param interrupted whether the command was interrupted/canceled
-   */
-  virtual void End(bool interrupted);
-
-  /**
-   * Whether the command has finished.  Once a command finishes, the scheduler
-   * will call its end() method and un-schedule it.
-   *
-   * @return whether the command has finished.
-   */
-  virtual bool IsFinished() { return false; }
-
-  /**
-   * Specifies the set of subsystems used by this command.  Two commands cannot
-   * use the same subsystem at the same time.  If the command is scheduled as
-   * interruptible and another command is scheduled that shares a requirement,
-   * the command will be interrupted.  Else, the command will not be scheduled.
-   * If no subsystems are required, return an empty set.
-   *
-   * <p>Note: it is recommended that user implementations contain the
-   * requirements as a field, and return that field here, rather than allocating
-   * a new set every time this is called.
-   *
-   * @return the set of subsystems that are required
-   */
-  virtual wpi::SmallSet<Subsystem*, 4> GetRequirements() const = 0;
-
-  /**
-   * Decorates this command with a timeout.  If the specified timeout is
-   * exceeded before the command finishes normally, the command will be
-   * interrupted and un-scheduled.  Note that the timeout only applies to the
-   * command returned by this method; the calling command is not itself changed.
-   *
-   * @param duration the timeout duration
-   * @return the command with the timeout added
-   */
-  ParallelRaceGroup WithTimeout(units::second_t duration) &&;
-
-  /**
-   * Decorates this command with an interrupt condition.  If the specified
-   * condition becomes true before the command finishes normally, the command
-   * will be interrupted and un-scheduled. Note that this only applies to the
-   * command returned by this method; the calling command is not itself changed.
-   *
-   * @param condition the interrupt condition
-   * @return the command with the interrupt condition added
-   */
-  ParallelRaceGroup WithInterrupt(std::function<bool()> condition) &&;
-
-  /**
-   * Decorates this command with a runnable to run before this command starts.
-   *
-   * @param toRun the Runnable to run
-   * @return the decorated command
-   */
-  SequentialCommandGroup BeforeStarting(std::function<void()> toRun) &&;
-
-  /**
-   * Decorates this command with a runnable to run after the command finishes.
-   *
-   * @param toRun the Runnable to run
-   * @return the decorated command
-   */
-  SequentialCommandGroup AndThen(std::function<void()> toRun) &&;
-
-  /**
-   * Decorates this command to run perpetually, ignoring its ordinary end
-   * conditions.  The decorated command can still be interrupted or canceled.
-   *
-   * @return the decorated command
-   */
-  PerpetualCommand Perpetually() &&;
-
-  /**
-   * Decorates this command to run "by proxy" by wrapping it in a {@link
-   * ProxyScheduleCommand}. This is useful for "forking off" from command groups
-   * when the user does not wish to extend the command's requirements to the
-   * entire command group.
-   *
-   * @return the decorated command
-   */
-  ProxyScheduleCommand AsProxy();
-
-  /**
-   * Schedules this command.
-   *
-   * @param interruptible whether this command can be interrupted by another
-   * command that shares one of its requirements
-   */
-  void Schedule(bool interruptible);
-
-  /**
-   * Schedules this command, defaulting to interruptible.
-   */
-  void Schedule() { Schedule(true); }
-
-  /**
-   * Cancels this command.  Will call the command's interrupted() method.
-   * Commands will be canceled even if they are not marked as interruptible.
-   */
-  void Cancel();
-
-  /**
-   * Whether or not the command is currently scheduled.  Note that this does not
-   * detect whether the command is being run by a CommandGroup, only whether it
-   * is directly being run by the scheduler.
-   *
-   * @return Whether the command is scheduled.
-   */
-  bool IsScheduled() const;
-
-  /**
-   * Whether the command requires a given subsystem.  Named "hasRequirement"
-   * rather than "requires" to avoid confusion with
-   * {@link
-   * edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)}
-   *  - this may be able to be changed in a few years.
-   *
-   * @param requirement the subsystem to inquire about
-   * @return whether the subsystem is required
-   */
-  bool HasRequirement(Subsystem* requirement) const;
-
-  /**
-   * Whether the command is currently grouped in a command group.  Used as extra
-   * insurance to prevent accidental independent use of grouped commands.
-   */
-  bool IsGrouped() const;
-
-  /**
-   * Sets whether the command is currently grouped in a command group.  Can be
-   * used to "reclaim" a command if a group is no longer going to use it.  NOT
-   * ADVISED!
-   */
-  void SetGrouped(bool grouped);
-
-  /**
-   * Whether the given command should run when the robot is disabled.  Override
-   * to return true if the command should run when disabled.
-   *
-   * @return whether the command should run when the robot is disabled
-   */
-  virtual bool RunsWhenDisabled() const { return false; }
-
-  virtual std::string GetName() const;
-
- protected:
-  /**
-   * Transfers ownership of this command to a unique pointer.  Used for
-   * decorator methods.
-   */
-  virtual std::unique_ptr<Command> TransferOwnership() && = 0;
-
-  bool m_isGrouped = false;
-};
-
-/**
- * Checks if two commands have disjoint requirement sets.
- *
- * @param first The first command to check.
- * @param second The second command to check.
- * @return False if first and second share a requirement.
- */
-bool RequirementsDisjoint(Command* first, Command* second);
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandBase.h b/wpilibc/src/main/native/include/frc2/command/CommandBase.h
deleted file mode 100644
index 3b1698f..0000000
--- a/wpilibc/src/main/native/include/frc2/command/CommandBase.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
-
-#include <string>
-
-#include <wpi/SmallSet.h>
-#include <wpi/Twine.h>
-
-#include "Command.h"
-
-namespace frc2 {
-/**
- * A Sendable base class for Commands.
- */
-class CommandBase : public Command,
-                    public frc::Sendable,
-                    public frc::SendableHelper<CommandBase> {
- public:
-  /**
-   * Adds the specified requirements to the command.
-   *
-   * @param requirements the requirements to add
-   */
-  void AddRequirements(std::initializer_list<Subsystem*> requirements);
-
-  void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
-
-  wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
-
-  /**
-   * Sets the name of this Command.
-   *
-   * @param name name
-   */
-  void SetName(const wpi::Twine& name);
-
-  /**
-   * Gets the name of this Command.
-   *
-   * @return Name
-   */
-  std::string GetName() const override;
-
-  /**
-   * Gets the subsystem name of this Command.
-   *
-   * @return Subsystem name
-   */
-  std::string GetSubsystem() const;
-
-  /**
-   * Sets the subsystem name of this Command.
-   *
-   * @param subsystem subsystem name
-   */
-  void SetSubsystem(const wpi::Twine& subsystem);
-
-  void InitSendable(frc::SendableBuilder& builder) override;
-
- protected:
-  CommandBase();
-  wpi::SmallSet<Subsystem*, 4> m_requirements;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h b/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h
deleted file mode 100644
index 9d265b4..0000000
--- a/wpilibc/src/main/native/include/frc2/command/CommandGroupBase.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/ErrorBase.h>
-
-#include <memory>
-#include <set>
-#include <vector>
-
-#include "CommandBase.h"
-
-namespace frc2 {
-
-/**
- * A base for CommandGroups.  Statically tracks commands that have been
- * allocated to groups to ensure those commands are not also used independently,
- * which can result in inconsistent command state and unpredictable execution.
- */
-class CommandGroupBase : public CommandBase {
- public:
-  /**
-   * Requires that the specified command not have been already allocated to a
-   * CommandGroup. Reports an error if the command is already grouped.
-   *
-   * @param commands The command to check
-   * @return True if all the command is ungrouped.
-   */
-  static bool RequireUngrouped(Command& command);
-
-  /**
-   * Requires that the specified commands not have been already allocated to a
-   * CommandGroup. Reports an error if any of the commands are already grouped.
-   *
-   * @param commands The commands to check
-   * @return True if all the commands are ungrouped.
-   */
-  static bool RequireUngrouped(wpi::ArrayRef<std::unique_ptr<Command>>);
-
-  /**
-   * Requires that the specified commands not have been already allocated to a
-   * CommandGroup. Reports an error if any of the commands are already grouped.
-   *
-   * @param commands The commands to check
-   * @return True if all the commands are ungrouped.
-   */
-  static bool RequireUngrouped(std::initializer_list<Command*>);
-
-  /**
-   * Adds the given commands to the command group.
-   *
-   * @param commands The commands to add.
-   */
-  virtual void AddCommands(
-      std::vector<std::unique_ptr<Command>>&& commands) = 0;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandHelper.h b/wpilibc/src/main/native/include/frc2/command/CommandHelper.h
deleted file mode 100644
index ec15f88..0000000
--- a/wpilibc/src/main/native/include/frc2/command/CommandHelper.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <type_traits>
-#include <utility>
-
-#include "Command.h"
-
-namespace frc2 {
-
-/**
- * CRTP implementation to allow polymorphic decorator functions in Command.
- *
- * <p>Note: ALWAYS create a subclass by extending CommandHelper<Base, Subclass>,
- * or decorators will not function!
- */
-template <typename Base, typename CRTP,
-          typename = std::enable_if_t<std::is_base_of_v<Command, Base>>>
-class CommandHelper : public Base {
-  using Base::Base;
-
- public:
-  CommandHelper() = default;
-
- protected:
-  std::unique_ptr<Command> TransferOwnership() && override {
-    return std::make_unique<CRTP>(std::move(*static_cast<CRTP*>(this)));
-  }
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h b/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h
deleted file mode 100644
index 2e9cdd5..0000000
--- a/wpilibc/src/main/native/include/frc2/command/CommandScheduler.h
+++ /dev/null
@@ -1,372 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/ErrorBase.h>
-#include <frc/RobotState.h>
-#include <frc/WPIErrors.h>
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-
-#include <networktables/NetworkTableEntry.h>
-#include <wpi/DenseMap.h>
-#include <wpi/FunctionExtras.h>
-#include <wpi/SmallSet.h>
-
-#include "CommandState.h"
-
-namespace frc2 {
-class Command;
-class Subsystem;
-
-/**
- * The scheduler responsible for running Commands.  A Command-based robot should
- * call Run() on the singleton instance in its periodic block in order to run
- * commands synchronously from the main loop.  Subsystems should be registered
- * with the scheduler using RegisterSubsystem() in order for their Periodic()
- * methods to be called and for their default commands to be scheduled.
- */
-class CommandScheduler final : public frc::Sendable,
-                               public frc::ErrorBase,
-                               public frc::SendableHelper<CommandScheduler> {
- public:
-  /**
-   * Returns the Scheduler instance.
-   *
-   * @return the instance
-   */
-  static CommandScheduler& GetInstance();
-
-  using Action = std::function<void(const Command&)>;
-
-  /**
-   * Adds a button binding to the scheduler, which will be polled to schedule
-   * commands.
-   *
-   * @param button The button to add
-   */
-  void AddButton(wpi::unique_function<void()> button);
-
-  /**
-   * Removes all button bindings from the scheduler.
-   */
-  void ClearButtons();
-
-  /**
-   * Schedules a command for execution.  Does nothing if the command is already
-   * scheduled. If a command's requirements are not available, it will only be
-   * started if all the commands currently using those requirements have been
-   * scheduled as interruptible.  If this is the case, they will be interrupted
-   * and the command will be scheduled.
-   *
-   * @param interruptible whether this command can be interrupted
-   * @param command       the command to schedule
-   */
-  void Schedule(bool interruptible, Command* command);
-
-  /**
-   * Schedules a command for execution, with interruptible defaulted to true.
-   * Does nothing if the command is already scheduled.
-   *
-   * @param command the command to schedule
-   */
-  void Schedule(Command* command);
-
-  /**
-   * Schedules multiple commands for execution.  Does nothing if the command is
-   * already scheduled. If a command's requirements are not available, it will
-   * only be started if all the commands currently using those requirements have
-   * been scheduled as interruptible.  If this is the case, they will be
-   * interrupted and the command will be scheduled.
-   *
-   * @param interruptible whether the commands should be interruptible
-   * @param commands      the commands to schedule
-   */
-  void Schedule(bool interruptible, wpi::ArrayRef<Command*> commands);
-
-  /**
-   * Schedules multiple commands for execution.  Does nothing if the command is
-   * already scheduled. If a command's requirements are not available, it will
-   * only be started if all the commands currently using those requirements have
-   * been scheduled as interruptible.  If this is the case, they will be
-   * interrupted and the command will be scheduled.
-   *
-   * @param interruptible whether the commands should be interruptible
-   * @param commands      the commands to schedule
-   */
-  void Schedule(bool interruptible, std::initializer_list<Command*> commands);
-
-  /**
-   * Schedules multiple commands for execution, with interruptible defaulted to
-   * true.  Does nothing if the command is already scheduled.
-   *
-   * @param commands the commands to schedule
-   */
-  void Schedule(wpi::ArrayRef<Command*> commands);
-
-  /**
-   * Schedules multiple commands for execution, with interruptible defaulted to
-   * true.  Does nothing if the command is already scheduled.
-   *
-   * @param commands the commands to schedule
-   */
-  void Schedule(std::initializer_list<Command*> commands);
-
-  /**
-   * Runs a single iteration of the scheduler.  The execution occurs in the
-   * following order:
-   *
-   * <p>Subsystem periodic methods are called.
-   *
-   * <p>Button bindings are polled, and new commands are scheduled from them.
-   *
-   * <p>Currently-scheduled commands are executed.
-   *
-   * <p>End conditions are checked on currently-scheduled commands, and commands
-   * that are finished have their end methods called and are removed.
-   *
-   * <p>Any subsystems not being used as requirements have their default methods
-   * started.
-   */
-  void Run();
-
-  /**
-   * Registers subsystems with the scheduler.  This must be called for the
-   * subsystem's periodic block to run when the scheduler is run, and for the
-   * subsystem's default command to be scheduled.  It is recommended to call
-   * this from the constructor of your subsystem implementations.
-   *
-   * @param subsystem the subsystem to register
-   */
-  void RegisterSubsystem(Subsystem* subsystem);
-
-  /**
-   * Un-registers subsystems with the scheduler.  The subsystem will no longer
-   * have its periodic block called, and will not have its default command
-   * scheduled.
-   *
-   * @param subsystem the subsystem to un-register
-   */
-  void UnregisterSubsystem(Subsystem* subsystem);
-
-  void RegisterSubsystem(std::initializer_list<Subsystem*> subsystems);
-
-  void UnregisterSubsystem(std::initializer_list<Subsystem*> subsystems);
-
-  /**
-   * Sets the default command for a subsystem.  Registers that subsystem if it
-   * is not already registered.  Default commands will run whenever there is no
-   * other command currently scheduled that requires the subsystem.  Default
-   * commands should be written to never end (i.e. their IsFinished() method
-   * should return false), as they would simply be re-scheduled if they do.
-   * Default commands must also require their subsystem.
-   *
-   * @param subsystem      the subsystem whose default command will be set
-   * @param defaultCommand the default command to associate with the subsystem
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) {
-    if (!defaultCommand.HasRequirement(subsystem)) {
-      wpi_setWPIErrorWithContext(
-          CommandIllegalUse, "Default commands must require their subsystem!");
-      return;
-    }
-    if (defaultCommand.IsFinished()) {
-      wpi_setWPIErrorWithContext(CommandIllegalUse,
-                                 "Default commands should not end!");
-      return;
-    }
-    m_subsystems[subsystem] = std::make_unique<std::remove_reference_t<T>>(
-        std::forward<T>(defaultCommand));
-  }
-
-  /**
-   * Gets the default command associated with this subsystem.  Null if this
-   * subsystem has no default command associated with it.
-   *
-   * @param subsystem the subsystem to inquire about
-   * @return the default command associated with the subsystem
-   */
-  Command* GetDefaultCommand(const Subsystem* subsystem) const;
-
-  /**
-   * Cancels a command.  The scheduler will only call the interrupted method of
-   * a canceled command, not the end method (though the interrupted method may
-   * itself call the end method).  Commands will be canceled even if they are
-   * not scheduled as interruptible.
-   *
-   * @param command the command to cancel
-   */
-  void Cancel(Command* command);
-
-  /**
-   * Cancels commands.  The scheduler will only call the interrupted method of a
-   * canceled command, not the end method (though the interrupted method may
-   * itself call the end method).  Commands will be canceled even if they are
-   * not scheduled as interruptible.
-   *
-   * @param commands the commands to cancel
-   */
-  void Cancel(wpi::ArrayRef<Command*> commands);
-
-  /**
-   * Cancels commands.  The scheduler will only call the interrupted method of a
-   * canceled command, not the end method (though the interrupted method may
-   * itself call the end method).  Commands will be canceled even if they are
-   * not scheduled as interruptible.
-   *
-   * @param commands the commands to cancel
-   */
-  void Cancel(std::initializer_list<Command*> commands);
-
-  /**
-   * Cancels all commands that are currently scheduled.
-   */
-  void CancelAll();
-
-  /**
-   * Returns the time since a given command was scheduled.  Note that this only
-   * works on commands that are directly scheduled by the scheduler; it will not
-   * work on commands inside of commandgroups, as the scheduler does not see
-   * them.
-   *
-   * @param command the command to query
-   * @return the time since the command was scheduled, in seconds
-   */
-  double TimeSinceScheduled(const Command* command) const;
-
-  /**
-   * Whether the given commands are running.  Note that this only works on
-   * commands that are directly scheduled by the scheduler; it will not work on
-   * commands inside of CommandGroups, as the scheduler does not see them.
-   *
-   * @param commands the command to query
-   * @return whether the command is currently scheduled
-   */
-  bool IsScheduled(wpi::ArrayRef<const Command*> commands) const;
-
-  /**
-   * Whether the given commands are running.  Note that this only works on
-   * commands that are directly scheduled by the scheduler; it will not work on
-   * commands inside of CommandGroups, as the scheduler does not see them.
-   *
-   * @param commands the command to query
-   * @return whether the command is currently scheduled
-   */
-  bool IsScheduled(std::initializer_list<const Command*> commands) const;
-
-  /**
-   * Whether a given command is running.  Note that this only works on commands
-   * that are directly scheduled by the scheduler; it will not work on commands
-   * inside of CommandGroups, as the scheduler does not see them.
-   *
-   * @param commands the command to query
-   * @return whether the command is currently scheduled
-   */
-  bool IsScheduled(const Command* command) const;
-
-  /**
-   * Returns the command currently requiring a given subsystem.  Null if no
-   * command is currently requiring the subsystem
-   *
-   * @param subsystem the subsystem to be inquired about
-   * @return the command currently requiring the subsystem
-   */
-  Command* Requiring(const Subsystem* subsystem) const;
-
-  /**
-   * Disables the command scheduler.
-   */
-  void Disable();
-
-  /**
-   * Enables the command scheduler.
-   */
-  void Enable();
-
-  /**
-   * Adds an action to perform on the initialization of any command by the
-   * scheduler.
-   *
-   * @param action the action to perform
-   */
-  void OnCommandInitialize(Action action);
-
-  /**
-   * Adds an action to perform on the execution of any command by the scheduler.
-   *
-   * @param action the action to perform
-   */
-  void OnCommandExecute(Action action);
-
-  /**
-   * Adds an action to perform on the interruption of any command by the
-   * scheduler.
-   *
-   * @param action the action to perform
-   */
-  void OnCommandInterrupt(Action action);
-
-  /**
-   * Adds an action to perform on the finishing of any command by the scheduler.
-   *
-   * @param action the action to perform
-   */
-  void OnCommandFinish(Action action);
-
-  void InitSendable(frc::SendableBuilder& builder) override;
-
- private:
-  // Constructor; private as this is a singleton
-  CommandScheduler();
-
-  // A map from commands to their scheduling state.  Also used as a set of the
-  // currently-running commands.
-  wpi::DenseMap<Command*, CommandState> m_scheduledCommands;
-
-  // A map from required subsystems to their requiring commands.  Also used as a
-  // set of the currently-required subsystems.
-  wpi::DenseMap<Subsystem*, Command*> m_requirements;
-
-  // A map from subsystems registered with the scheduler to their default
-  // commands.  Also used as a list of currently-registered subsystems.
-  wpi::DenseMap<Subsystem*, std::unique_ptr<Command>> m_subsystems;
-
-  // The set of currently-registered buttons that will be polled every
-  // iteration.
-  wpi::SmallVector<wpi::unique_function<void()>, 4> m_buttons;
-
-  bool m_disabled{false};
-
-  // NetworkTable entries for use in Sendable impl
-  nt::NetworkTableEntry m_namesEntry;
-  nt::NetworkTableEntry m_idsEntry;
-  nt::NetworkTableEntry m_cancelEntry;
-
-  // Lists of user-supplied actions to be executed on scheduling events for
-  // every command.
-  wpi::SmallVector<Action, 4> m_initActions;
-  wpi::SmallVector<Action, 4> m_executeActions;
-  wpi::SmallVector<Action, 4> m_interruptActions;
-  wpi::SmallVector<Action, 4> m_finishActions;
-
-  // Flag and queues for avoiding concurrent modification if commands are
-  // scheduled/canceled during run
-
-  bool m_inRunLoop = false;
-  wpi::DenseMap<Command*, bool> m_toSchedule;
-  wpi::SmallVector<Command*, 4> m_toCancel;
-
-  friend class CommandTestBase;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/CommandState.h b/wpilibc/src/main/native/include/frc2/command/CommandState.h
deleted file mode 100644
index 41fc5d0..0000000
--- a/wpilibc/src/main/native/include/frc2/command/CommandState.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace frc2 {
-/**
- * Class that holds scheduling state for a command.  Used internally by the
- * CommandScheduler
- */
-class CommandState final {
- public:
-  CommandState() = default;
-
-  explicit CommandState(bool interruptible);
-
-  bool IsInterruptible() const { return m_interruptible; }
-
-  // The time since this command was initialized.
-  double TimeSinceInitialized() const;
-
- private:
-  double m_startTime = -1;
-  bool m_interruptible;
-
-  void StartTiming();
-  void StartRunning();
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h b/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h
deleted file mode 100644
index 0419cf9..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ConditionalCommand.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <iostream>
-#include <memory>
-#include <utility>
-
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * Runs one of two commands, depending on the value of the given condition when
- * this command is initialized.  Does not actually schedule the selected command
- * - rather, the command is run through this command; this ensures that the
- * command will behave as expected if used as part of a CommandGroup.  Requires
- * the requirements of both commands, again to ensure proper functioning when
- * used in a CommandGroup.  If this is undesired, consider using
- * ScheduleCommand.
- *
- * <p>As this command contains multiple component commands within it, it is
- * technically a command group; the command instances that are passed to it
- * cannot be added to any other groups, or scheduled individually.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- *
- * @see ScheduleCommand
- */
-class ConditionalCommand
-    : public CommandHelper<CommandBase, ConditionalCommand> {
- public:
-  /**
-   * Creates a new ConditionalCommand.
-   *
-   * @param onTrue    the command to run if the condition is true
-   * @param onFalse   the command to run if the condition is false
-   * @param condition the condition to determine which command to run
-   */
-  template <class T1, class T2,
-            typename = std::enable_if_t<
-                std::is_base_of_v<Command, std::remove_reference_t<T1>>>,
-            typename = std::enable_if_t<
-                std::is_base_of_v<Command, std::remove_reference_t<T2>>>>
-  ConditionalCommand(T1&& onTrue, T2&& onFalse, std::function<bool()> condition)
-      : ConditionalCommand(std::make_unique<std::remove_reference_t<T1>>(
-                               std::forward<T1>(onTrue)),
-                           std::make_unique<std::remove_reference_t<T2>>(
-                               std::forward<T2>(onFalse)),
-                           condition) {}
-
-  /**
-   * Creates a new ConditionalCommand.
-   *
-   * @param onTrue    the command to run if the condition is true
-   * @param onFalse   the command to run if the condition is false
-   * @param condition the condition to determine which command to run
-   */
-  ConditionalCommand(std::unique_ptr<Command>&& onTrue,
-                     std::unique_ptr<Command>&& onFalse,
-                     std::function<bool()> condition);
-
-  ConditionalCommand(ConditionalCommand&& other) = default;
-
-  // No copy constructors for command groups
-  ConditionalCommand(const ConditionalCommand& other) = delete;
-
-  void Initialize() override;
-
-  void Execute() override;
-
-  void End(bool interrupted) override;
-
-  bool IsFinished() override;
-
-  bool RunsWhenDisabled() const override;
-
- private:
-  std::unique_ptr<Command> m_onTrue;
-  std::unique_ptr<Command> m_onFalse;
-  std::function<bool()> m_condition;
-  Command* m_selectedCommand{nullptr};
-  bool m_runsWhenDisabled = true;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h b/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h
deleted file mode 100644
index b47135c..0000000
--- a/wpilibc/src/main/native/include/frc2/command/FunctionalCommand.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that allows the user to pass in functions for each of the basic
- * command methods through the constructor.  Useful for inline definitions of
- * complex commands - note, however, that if a command is beyond a certain
- * complexity it is usually better practice to write a proper class for it than
- * to inline it.
- */
-class FunctionalCommand : public CommandHelper<CommandBase, FunctionalCommand> {
- public:
-  /**
-   * Creates a new FunctionalCommand.
-   *
-   * @param onInit       the function to run on command initialization
-   * @param onExecute    the function to run on command execution
-   * @param onEnd        the function to run on command end
-   * @param isFinished   the function that determines whether the command has
-   * finished
-   * @param requirements the subsystems required by this command
-   */
-  FunctionalCommand(std::function<void()> onInit,
-                    std::function<void()> onExecute,
-                    std::function<void(bool)> onEnd,
-                    std::function<bool()> isFinished);
-
-  FunctionalCommand(FunctionalCommand&& other) = default;
-
-  FunctionalCommand(const FunctionalCommand& other) = default;
-
-  void Initialize() override;
-
-  void Execute() override;
-
-  void End(bool interrupted) override;
-
-  bool IsFinished() override;
-
- private:
-  std::function<void()> m_onInit;
-  std::function<void()> m_onExecute;
-  std::function<void(bool)> m_onEnd;
-  std::function<bool()> m_isFinished;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/InstantCommand.h b/wpilibc/src/main/native/include/frc2/command/InstantCommand.h
deleted file mode 100644
index ec28a11..0000000
--- a/wpilibc/src/main/native/include/frc2/command/InstantCommand.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A Command that runs instantly; it will initialize, execute once, and end on
- * the same iteration of the scheduler.  Users can either pass in a Runnable and
- * a set of requirements, or else subclass this command if desired.
- */
-class InstantCommand : public CommandHelper<CommandBase, InstantCommand> {
- public:
-  /**
-   * Creates a new InstantCommand that runs the given Runnable with the given
-   * requirements.
-   *
-   * @param toRun        the Runnable to run
-   * @param requirements the subsystems required by this command
-   */
-  InstantCommand(std::function<void()> toRun,
-                 std::initializer_list<Subsystem*> requirements);
-
-  InstantCommand(InstantCommand&& other) = default;
-
-  InstantCommand(const InstantCommand& other) = default;
-
-  /**
-   * Creates a new InstantCommand with a Runnable that does nothing.  Useful
-   * only as a no-arg constructor to call implicitly from subclass constructors.
-   */
-  InstantCommand();
-
-  void Initialize() override;
-
-  bool IsFinished() final;
-
- private:
-  std::function<void()> m_toRun;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h b/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h
deleted file mode 100644
index 3365c04..0000000
--- a/wpilibc/src/main/native/include/frc2/command/NotifierCommand.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/Notifier.h>
-
-#include <units/units.h>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that starts a notifier to run the given runnable periodically in a
- * separate thread. Has no end condition as-is; either subclass it or use {@link
- * Command#withTimeout(double)} or
- * {@link Command#withInterrupt(BooleanSupplier)} to give it one.
- *
- * <p>WARNING: Do not use this class unless you are confident in your ability to
- * make the executed code thread-safe.  If you do not know what "thread-safe"
- * means, that is a good sign that you should not use this class.
- */
-class NotifierCommand : public CommandHelper<CommandBase, NotifierCommand> {
- public:
-  /**
-   * Creates a new NotifierCommand.
-   *
-   * @param toRun        the runnable for the notifier to run
-   * @param period       the period at which the notifier should run
-   * @param requirements the subsystems required by this command
-   */
-  NotifierCommand(std::function<void()> toRun, units::second_t period,
-                  std::initializer_list<Subsystem*> requirements);
-
-  NotifierCommand(NotifierCommand&& other);
-
-  NotifierCommand(const NotifierCommand& other);
-
-  void Initialize() override;
-
-  void End(bool interrupted) override;
-
- private:
-  std::function<void()> m_toRun;
-  frc::Notifier m_notifier;
-  units::second_t m_period;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/PIDCommand.h b/wpilibc/src/main/native/include/frc2/command/PIDCommand.h
deleted file mode 100644
index 6e553ff..0000000
--- a/wpilibc/src/main/native/include/frc2/command/PIDCommand.h
+++ /dev/null
@@ -1,124 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/controller/PIDController.h"
-#include "frc2/command/CommandBase.h"
-#include "frc2/command/CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that controls an output with a PIDController.  Runs forever by
- * default - to add exit conditions and/or other behavior, subclass this class.
- * The controller calculation and output are performed synchronously in the
- * command's execute() method.
- *
- * @see PIDController
- */
-class PIDCommand : public CommandHelper<CommandBase, PIDCommand> {
- public:
-  /**
-   * Creates a new PIDCommand, which controls the given output with a
-   * PIDController.
-   *
-   * @param controller        the controller that controls the output.
-   * @param measurementSource the measurement of the process variable
-   * @param setpointSource   the controller's reference (aka setpoint)
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
-   */
-  PIDCommand(PIDController controller,
-             std::function<double()> measurementSource,
-             std::function<double()> setpointSource,
-             std::function<void(double)> useOutput,
-             std::initializer_list<Subsystem*> requirements);
-
-  /**
-   * Creates a new PIDCommand, which controls the given output with a
-   * PIDController with a constant setpoint.
-   *
-   * @param controller        the controller that controls the output.
-   * @param measurementSource the measurement of the process variable
-   * @param setpoint         the controller's setpoint (aka setpoint)
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
-   */
-  PIDCommand(PIDController controller,
-             std::function<double()> measurementSource, double setpoint,
-             std::function<void(double)> useOutput,
-             std::initializer_list<Subsystem*> requirements);
-
-  PIDCommand(PIDCommand&& other) = default;
-
-  PIDCommand(const PIDCommand& other) = default;
-
-  void Initialize() override;
-
-  void Execute() override;
-
-  void End(bool interrupted) override;
-
-  /**
-   * Sets the function that uses the output of the PIDController.
-   *
-   * @param useOutput The function that uses the output.
-   */
-  void SetOutput(std::function<void(double)> useOutput);
-
-  /**
-   * Sets the setpoint for the controller to track the given source.
-   *
-   * @param setpointSource The setpoint source
-   */
-  void SetSetpoint(std::function<double()> setpointSource);
-
-  /**
-   * Sets the setpoint for the controller to a constant value.
-   *
-   * @param setpoint The setpoint
-   */
-  void SetSetpoint(double setpoint);
-
-  /**
-   * Sets the setpoint for the controller to a constant value relative (i.e.
-   * added to) the current setpoint.
-   *
-   * @param relativeReference The change in setpoint
-   */
-  void SetSetpointRelative(double relativeSetpoint);
-
-  /**
-   * Gets the measurement of the process variable. Wraps the passed-in function
-   * for readability.
-   *
-   * @return The measurement of the process variable
-   */
-  virtual double GetMeasurement();
-
-  /**
-   * Gets the measurement of the process variable. Wraps the passed-in function
-   * for readability.
-   *
-   * @return The measurement of the process variable
-   */
-  virtual void UseOutput(double output);
-
-  /**
-   * Returns the PIDController used by the command.
-   *
-   * @return The PIDController
-   */
-  PIDController& getController();
-
- protected:
-  PIDController m_controller;
-  std::function<double()> m_measurement;
-  std::function<double()> m_setpoint;
-  std::function<void(double)> m_useOutput;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h b/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h
deleted file mode 100644
index 7aa21c0..0000000
--- a/wpilibc/src/main/native/include/frc2/command/PIDSubsystem.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/controller/PIDController.h"
-#include "frc2/command/SubsystemBase.h"
-
-namespace frc2 {
-/**
- * A subsystem that uses a PIDController to control an output.  The controller
- * is run synchronously from the subsystem's periodic() method.
- *
- * @see PIDController
- */
-class PIDSubsystem : public SubsystemBase {
- public:
-  /**
-   * Creates a new PIDSubsystem.
-   *
-   * @param controller the PIDController to use
-   */
-  explicit PIDSubsystem(PIDController controller);
-
-  void Periodic() override;
-
-  /**
-   * Uses the output from the PIDController.
-   *
-   * @param output the output of the PIDController
-   */
-  virtual void UseOutput(double output) = 0;
-
-  /**
-   * Returns the reference (setpoint) used by the PIDController.
-   *
-   * @return the reference (setpoint) to be used by the controller
-   */
-  virtual double GetSetpoint() = 0;
-
-  /**
-   * Returns the measurement of the process variable used by the PIDController.
-   *
-   * @return the measurement of the process variable
-   */
-  virtual double GetMeasurement() = 0;
-
-  /**
-   * Enables the PID control.  Resets the controller.
-   */
-  virtual void Enable();
-
-  /**
-   * Disables the PID control.  Sets output to zero.
-   */
-  virtual void Disable();
-
-  /**
-   * Returns the PIDController.
-   *
-   * @return The controller.
-   */
-  PIDController& GetController();
-
- protected:
-  PIDController m_controller;
-  bool m_enabled;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h
deleted file mode 100644
index 322e7b1..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ParallelCommandGroup.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-#include <vector>
-
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A CommandGroup that runs a set of commands in parallel, ending when the last
- * command ends.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class ParallelCommandGroup
-    : public CommandHelper<CommandGroupBase, ParallelCommandGroup> {
- public:
-  /**
-   * Creates a new ParallelCommandGroup.  The given commands will be executed
-   * simultaneously. The command group will finish when the last command
-   * finishes.  If the CommandGroup is interrupted, only the commands that are
-   * still running will be interrupted.
-   *
-   * @param commands the commands to include in this group.
-   */
-  explicit ParallelCommandGroup(
-      std::vector<std::unique_ptr<Command>>&& commands);
-
-  /**
-   * Creates a new ParallelCommandGroup.  The given commands will be executed
-   * simultaneously. The command group will finish when the last command
-   * finishes.  If the CommandGroup is interrupted, only the commands that are
-   * still running will be interrupted.
-   *
-   * @param commands the commands to include in this group.
-   */
-  template <class... Types,
-            typename = std::enable_if_t<std::conjunction_v<
-                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
-  explicit ParallelCommandGroup(Types&&... commands) {
-    AddCommands(std::forward<Types>(commands)...);
-  }
-
-  ParallelCommandGroup(ParallelCommandGroup&& other) = default;
-
-  // No copy constructors for commandgroups
-  ParallelCommandGroup(const ParallelCommandGroup&) = delete;
-
-  // Prevent template expansion from emulating copy ctor
-  ParallelCommandGroup(ParallelCommandGroup&) = delete;
-
-  template <class... Types,
-            typename = std::enable_if_t<std::conjunction_v<
-                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
-  void AddCommands(Types&&... commands) {
-    std::vector<std::unique_ptr<Command>> foo;
-    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
-         std::forward<Types>(commands))),
-     ...);
-    AddCommands(std::move(foo));
-  }
-
-  void Initialize() override;
-
-  void Execute() override;
-
-  void End(bool interrupted) override;
-
-  bool IsFinished() override;
-
-  bool RunsWhenDisabled() const override;
-
- private:
-  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
-
-  std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
-  bool m_runWhenDisabled{true};
-  bool isRunning = false;
-};
-}  // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
deleted file mode 100644
index 168d0f8..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-#include <vector>
-
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A CommandGroup that runs a set of commands in parallel, ending only when a
- * specific command (the "deadline") ends, interrupting all other commands that
- * are still running at that point.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class ParallelDeadlineGroup
-    : public CommandHelper<CommandGroupBase, ParallelDeadlineGroup> {
- public:
-  /**
-   * Creates a new ParallelDeadlineGroup.  The given commands (including the
-   * deadline) will be executed simultaneously.  The CommandGroup will finish
-   * when the deadline finishes, interrupting all other still-running commands.
-   * If the CommandGroup is interrupted, only the commands still running will be
-   * interrupted.
-   *
-   * @param deadline the command that determines when the group ends
-   * @param commands the commands to be executed
-   */
-  ParallelDeadlineGroup(std::unique_ptr<Command>&& deadline,
-                        std::vector<std::unique_ptr<Command>>&& commands);
-  /**
-   * Creates a new ParallelDeadlineGroup.  The given commands (including the
-   * deadline) will be executed simultaneously.  The CommandGroup will finish
-   * when the deadline finishes, interrupting all other still-running commands.
-   * If the CommandGroup is interrupted, only the commands still running will be
-   * interrupted.
-   *
-   * @param deadline the command that determines when the group ends
-   * @param commands the commands to be executed
-   */
-  template <class T, class... Types,
-            typename = std::enable_if_t<
-                std::is_base_of_v<Command, std::remove_reference_t<T>>>,
-            typename = std::enable_if_t<std::conjunction_v<
-                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
-  explicit ParallelDeadlineGroup(T&& deadline, Types&&... commands) {
-    SetDeadline(std::make_unique<std::remove_reference_t<T>>(
-        std::forward<T>(deadline)));
-    AddCommands(std::forward<Types>(commands)...);
-  }
-
-  ParallelDeadlineGroup(ParallelDeadlineGroup&& other) = default;
-
-  // No copy constructors for command groups
-  ParallelDeadlineGroup(const ParallelDeadlineGroup&) = delete;
-
-  // Prevent template expansion from emulating copy ctor
-  ParallelDeadlineGroup(ParallelDeadlineGroup&) = delete;
-
-  template <class... Types,
-            typename = std::enable_if_t<std::conjunction_v<
-                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
-  void AddCommands(Types&&... commands) {
-    std::vector<std::unique_ptr<Command>> foo;
-    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
-         std::forward<Types>(commands))),
-     ...);
-    AddCommands(std::move(foo));
-  }
-
-  void Initialize() override;
-
-  void Execute() override;
-
-  void End(bool interrupted) override;
-
-  bool IsFinished() override;
-
-  bool RunsWhenDisabled() const override;
-
- private:
-  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
-
-  void SetDeadline(std::unique_ptr<Command>&& deadline);
-
-  std::unordered_map<std::unique_ptr<Command>, bool> m_commands;
-  Command* m_deadline;
-  bool m_runWhenDisabled{true};
-  bool isRunning = false;
-};
-}  // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h b/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h
deleted file mode 100644
index d7411e9..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ParallelRaceGroup.h
+++ /dev/null
@@ -1,90 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <memory>
-#include <set>
-#include <unordered_map>
-#include <utility>
-#include <vector>
-
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A CommandGroup that runs a set of commands in parallel, ending when any one
- * of the commands ends and interrupting all the others.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class ParallelRaceGroup
-    : public CommandHelper<CommandGroupBase, ParallelRaceGroup> {
- public:
-  /**
-   * Creates a new ParallelCommandRace.  The given commands will be executed
-   * simultaneously, and will "race to the finish" - the first command to finish
-   * ends the entire command, with all other commands being interrupted.
-   *
-   * @param commands the commands to include in this group.
-   */
-  explicit ParallelRaceGroup(std::vector<std::unique_ptr<Command>>&& commands);
-
-  template <class... Types,
-            typename = std::enable_if_t<std::conjunction_v<
-                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
-  explicit ParallelRaceGroup(Types&&... commands) {
-    AddCommands(std::forward<Types>(commands)...);
-  }
-
-  ParallelRaceGroup(ParallelRaceGroup&& other) = default;
-
-  // No copy constructors for command groups
-  ParallelRaceGroup(const ParallelRaceGroup&) = delete;
-
-  // Prevent template expansion from emulating copy ctor
-  ParallelRaceGroup(ParallelRaceGroup&) = delete;
-
-  template <class... Types>
-  void AddCommands(Types&&... commands) {
-    std::vector<std::unique_ptr<Command>> foo;
-    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
-         std::forward<Types>(commands))),
-     ...);
-    AddCommands(std::move(foo));
-  }
-
-  void Initialize() override;
-
-  void Execute() override;
-
-  void End(bool interrupted) override;
-
-  bool IsFinished() override;
-
-  bool RunsWhenDisabled() const override;
-
- private:
-  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
-
-  std::set<std::unique_ptr<Command>> m_commands;
-  bool m_runWhenDisabled{true};
-  bool m_finished{false};
-  bool isRunning = false;
-};
-}  // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h b/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h
deleted file mode 100644
index 3f3c9e7..0000000
--- a/wpilibc/src/main/native/include/frc2/command/PerpetualCommand.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <memory>
-#include <utility>
-
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that runs another command in perpetuity, ignoring that command's
- * end conditions.  While this class does not extend {@link CommandGroupBase},
- * it is still considered a CommandGroup, as it allows one to compose another
- * command within it; the command instances that are passed to it cannot be
- * added to any other groups, or scheduled individually.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class PerpetualCommand : public CommandHelper<CommandBase, PerpetualCommand> {
- public:
-  /**
-   * Creates a new PerpetualCommand.  Will run another command in perpetuity,
-   * ignoring that command's end conditions, unless this command itself is
-   * interrupted.
-   *
-   * @param command the command to run perpetually
-   */
-  explicit PerpetualCommand(std::unique_ptr<Command>&& command);
-
-  /**
-   * Creates a new PerpetualCommand.  Will run another command in perpetuity,
-   * ignoring that command's end conditions, unless this command itself is
-   * interrupted.
-   *
-   * @param command the command to run perpetually
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  explicit PerpetualCommand(T&& command)
-      : PerpetualCommand(std::make_unique<std::remove_reference_t<T>>(
-            std::forward<T>(command))) {}
-
-  PerpetualCommand(PerpetualCommand&& other) = default;
-
-  // No copy constructors for command groups
-  PerpetualCommand(const PerpetualCommand& other) = delete;
-
-  // Prevent template expansion from emulating copy ctor
-  PerpetualCommand(PerpetualCommand&) = delete;
-
-  void Initialize() override;
-
-  void Execute() override;
-
-  void End(bool interrupted) override;
-
- private:
-  std::unique_ptr<Command> m_command;
-};
-}  // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/PrintCommand.h b/wpilibc/src/main/native/include/frc2/command/PrintCommand.h
deleted file mode 100644
index fb420cb..0000000
--- a/wpilibc/src/main/native/include/frc2/command/PrintCommand.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/Twine.h>
-#include <wpi/raw_ostream.h>
-
-#include "CommandHelper.h"
-#include "InstantCommand.h"
-
-namespace frc2 {
-/**
- * A command that prints a string when initialized.
- */
-class PrintCommand : public CommandHelper<InstantCommand, PrintCommand> {
- public:
-  /**
-   * Creates a new a PrintCommand.
-   *
-   * @param message the message to print
-   */
-  explicit PrintCommand(const wpi::Twine& message);
-
-  PrintCommand(PrintCommand&& other) = default;
-
-  PrintCommand(const PrintCommand& other) = default;
-
-  bool RunsWhenDisabled() const override;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h
deleted file mode 100644
index b1ff9e6..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDCommand.h
+++ /dev/null
@@ -1,132 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/controller/ProfiledPIDController.h"
-#include "frc2/command/CommandBase.h"
-#include "frc2/command/CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that controls an output with a ProfiledPIDController.  Runs forever
- * by default - to add exit conditions and/or other behavior, subclass this
- * class. The controller calculation and output are performed synchronously in
- * the command's execute() method.
- *
- * @see ProfiledPIDController
- */
-class ProfiledPIDCommand
-    : public CommandHelper<CommandBase, ProfiledPIDCommand> {
-  using State = frc::TrapezoidProfile::State;
-
- public:
-  /**
-   * Creates a new PIDCommand, which controls the given output with a
-   * ProfiledPIDController.
-   *
-   * @param controller        the controller that controls the output.
-   * @param measurementSource the measurement of the process variable
-   * @param goalSource   the controller's goal
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
-   */
-  ProfiledPIDCommand(frc::ProfiledPIDController controller,
-                     std::function<units::meter_t()> measurementSource,
-                     std::function<State()> goalSource,
-                     std::function<void(double, State)> useOutput,
-                     std::initializer_list<Subsystem*> requirements);
-
-  /**
-   * Creates a new PIDCommand, which controls the given output with a
-   * ProfiledPIDController.
-   *
-   * @param controller        the controller that controls the output.
-   * @param measurementSource the measurement of the process variable
-   * @param goalSource   the controller's goal
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
-   */
-  ProfiledPIDCommand(frc::ProfiledPIDController controller,
-                     std::function<units::meter_t()> measurementSource,
-                     std::function<units::meter_t()> goalSource,
-                     std::function<void(double, State)> useOutput,
-                     std::initializer_list<Subsystem*> requirements);
-
-  /**
-   * Creates a new PIDCommand, which controls the given output with a
-   * ProfiledPIDController with a constant goal.
-   *
-   * @param controller        the controller that controls the output.
-   * @param measurementSource the measurement of the process variable
-   * @param goal         the controller's goal
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
-   */
-  ProfiledPIDCommand(frc::ProfiledPIDController controller,
-                     std::function<units::meter_t()> measurementSource,
-                     State goal, std::function<void(double, State)> useOutput,
-                     std::initializer_list<Subsystem*> requirements);
-
-  /**
-   * Creates a new PIDCommand, which controls the given output with a
-   * ProfiledPIDController with a constant goal.
-   *
-   * @param controller        the controller that controls the output.
-   * @param measurementSource the measurement of the process variable
-   * @param goal         the controller's goal
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
-   */
-  ProfiledPIDCommand(frc::ProfiledPIDController controller,
-                     std::function<units::meter_t()> measurementSource,
-                     units::meter_t goal,
-                     std::function<void(double, State)> useOutput,
-                     std::initializer_list<Subsystem*> requirements);
-
-  ProfiledPIDCommand(ProfiledPIDCommand&& other) = default;
-
-  ProfiledPIDCommand(const ProfiledPIDCommand& other) = default;
-
-  void Initialize() override;
-
-  void Execute() override;
-
-  void End(bool interrupted) override;
-
-  /**
-   * Gets the measurement of the process variable. Wraps the passed-in function
-   * for readability.
-   *
-   * @return The measurement of the process variable
-   */
-  virtual units::meter_t GetMeasurement();
-
-  /**
-   * Gets the measurement of the process variable. Wraps the passed-in function
-   * for readability.
-   *
-   * @return The measurement of the process variable
-   */
-  virtual void UseOutput(double output, State state);
-
-  /**
-   * Returns the ProfiledPIDController used by the command.
-   *
-   * @return The ProfiledPIDController
-   */
-  frc::ProfiledPIDController& GetController();
-
- protected:
-  frc::ProfiledPIDController m_controller;
-  std::function<units::meter_t()> m_measurement;
-  std::function<State()> m_goal;
-  std::function<void(double, State)> m_useOutput;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
deleted file mode 100644
index 4fa47b2..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/controller/ProfiledPIDController.h"
-#include "frc2/command/SubsystemBase.h"
-
-namespace frc2 {
-/**
- * A subsystem that uses a ProfiledPIDController to control an output.  The
- * controller is run synchronously from the subsystem's periodic() method.
- *
- * @see ProfiledPIDController
- */
-class ProfiledPIDSubsystem : public SubsystemBase {
-  using State = frc::TrapezoidProfile::State;
-
- public:
-  /**
-   * Creates a new ProfiledPIDSubsystem.
-   *
-   * @param controller the ProfiledPIDController to use
-   */
-  explicit ProfiledPIDSubsystem(frc::ProfiledPIDController controller);
-
-  void Periodic() override;
-
-  /**
-   * Uses the output from the ProfiledPIDController.
-   *
-   * @param output the output of the ProfiledPIDController
-   */
-  virtual void UseOutput(double output, State state) = 0;
-
-  /**
-   * Returns the goal used by the ProfiledPIDController.
-   *
-   * @return the goal to be used by the controller
-   */
-  virtual State GetGoal() = 0;
-
-  /**
-   * Returns the measurement of the process variable used by the
-   * ProfiledPIDController.
-   *
-   * @return the measurement of the process variable
-   */
-  virtual units::meter_t GetMeasurement() = 0;
-
-  /**
-   * Enables the PID control.  Resets the controller.
-   */
-  virtual void Enable();
-
-  /**
-   * Disables the PID control.  Sets output to zero.
-   */
-  virtual void Disable();
-
-  /**
-   * Returns the ProfiledPIDController.
-   *
-   * @return The controller.
-   */
-  frc::ProfiledPIDController& GetController();
-
- protected:
-  frc::ProfiledPIDController m_controller;
-  bool m_enabled;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h b/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h
deleted file mode 100644
index 8906424..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ProxyScheduleCommand.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/SmallVector.h>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "SetUtilities.h"
-
-namespace frc2 {
-/**
- * Schedules the given commands when this command is initialized, and ends when
- * all the commands are no longer scheduled.  Useful for forking off from
- * CommandGroups.  If this command is interrupted, it will cancel all of the
- * commands.
- */
-class ProxyScheduleCommand
-    : public CommandHelper<CommandBase, ProxyScheduleCommand> {
- public:
-  /**
-   * Creates a new ProxyScheduleCommand that schedules the given commands when
-   * initialized, and ends when they are all no longer scheduled.
-   *
-   * @param toSchedule the commands to schedule
-   */
-  explicit ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
-
-  ProxyScheduleCommand(ProxyScheduleCommand&& other) = default;
-
-  ProxyScheduleCommand(const ProxyScheduleCommand& other) = default;
-
-  void Initialize() override;
-
-  void End(bool interrupted) override;
-
-  void Execute() override;
-
-  bool IsFinished() override;
-
- private:
-  wpi::SmallVector<Command*, 4> m_toSchedule;
-  bool m_finished{false};
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h b/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h
deleted file mode 100644
index 592f194..0000000
--- a/wpilibc/src/main/native/include/frc2/command/RamseteCommand.h
+++ /dev/null
@@ -1,147 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <functional>
-#include <memory>
-
-#include <units/units.h>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "frc/controller/PIDController.h"
-#include "frc/controller/RamseteController.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/trajectory/Trajectory.h"
-#include "frc2/Timer.h"
-
-#pragma once
-
-namespace frc2 {
-/**
- * A command that uses a RAMSETE controller  to follow a trajectory
- * with a differential drive.
- *
- * <p>The command handles trajectory-following, PID calculations, and
- * feedforwards internally.  This is intended to be a more-or-less "complete
- * solution" that can be used by teams without a great deal of controls
- * expertise.
- *
- * <p>Advanced teams seeking more flexibility (for example, those who wish to
- * use the onboard PID functionality of a "smart" motor controller) may use the
- * secondary constructor that omits the PID and feedforward functionality,
- * returning only the raw wheel speeds from the RAMSETE controller.
- *
- * @see RamseteController
- * @see Trajectory
- */
-class RamseteCommand : public CommandHelper<CommandBase, RamseteCommand> {
-  using voltsecondspermeter =
-      units::compound_unit<units::volt, units::second,
-                           units::inverse<units::meter>>;
-  using voltsecondssquaredpermeter =
-      units::compound_unit<units::volt, units::squared<units::second>,
-                           units::inverse<units::meter>>;
-
- public:
-  /**
-   * Constructs a new RamseteCommand that, when executed, will follow the
-   * provided trajectory. PID control and feedforward are handled internally,
-   * and outputs are scaled -1 to 1 for easy consumption by speed controllers.
-   *
-   * <p>Note: The controller will *not* set the outputVolts to zero upon
-   * completion of the path - this is left to the user, since it is not
-   * appropriate for paths with nonstationary endstates.
-   *
-   * @param trajectory      The trajectory to follow.
-   * @param pose            A function that supplies the robot pose - use one of
-   * the odometry classes to provide this.
-   * @param controller      The RAMSETE controller used to follow the
-   * trajectory.
-   * @param ks              Constant feedforward term for the robot drive.
-   * @param kv              Velocity-proportional feedforward term for the robot
-   * drive.
-   * @param ka              Acceleration-proportional feedforward term for the
-   * robot drive.
-   * @param kinematics      The kinematics for the robot drivetrain.
-   * @param leftSpeed       A function that supplies the speed of the left side
-   * of the robot drive.
-   * @param rightSpeed      A function that supplies the speed of the right side
-   * of the robot drive.
-   * @param leftController  The PIDController for the left side of the robot
-   * drive.
-   * @param rightController The PIDController for the right side of the robot
-   * drive.
-   * @param output          A function that consumes the computed left and right
-   * outputs (in volts) for the robot drive.
-   * @param requirements    The subsystems to require.
-   */
-  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
-                 frc::RamseteController controller, units::volt_t ks,
-                 units::unit_t<voltsecondspermeter> kv,
-                 units::unit_t<voltsecondssquaredpermeter> ka,
-                 frc::DifferentialDriveKinematics kinematics,
-                 std::function<units::meters_per_second_t()> leftSpeed,
-                 std::function<units::meters_per_second_t()> rightSpeed,
-                 frc2::PIDController leftController,
-                 frc2::PIDController rightController,
-                 std::function<void(units::volt_t, units::volt_t)> output,
-                 std::initializer_list<Subsystem*> requirements);
-
-  /**
-   * Constructs a new RamseteCommand that, when executed, will follow the
-   * provided trajectory. Performs no PID control and calculates no
-   * feedforwards; outputs are the raw wheel speeds from the RAMSETE controller,
-   * and will need to be converted into a usable form by the user.
-   *
-   * @param trajectory      The trajectory to follow.
-   * @param pose            A function that supplies the robot pose - use one of
-   * the odometry classes to provide this.
-   * @param controller      The RAMSETE controller used to follow the
-   * trajectory.
-   * @param kinematics      The kinematics for the robot drivetrain.
-   * @param output          A function that consumes the computed left and right
-   * outputs (in volts) for the robot drive.
-   * @param requirements    The subsystems to require.
-   */
-  RamseteCommand(frc::Trajectory trajectory, std::function<frc::Pose2d()> pose,
-                 frc::RamseteController controller,
-                 frc::DifferentialDriveKinematics kinematics,
-                 std::function<void(units::meters_per_second_t,
-                                    units::meters_per_second_t)>
-                     output,
-                 std::initializer_list<Subsystem*> requirements);
-
-  void Initialize() override;
-
-  void Execute() override;
-
-  void End(bool interrupted) override;
-
-  bool IsFinished() override;
-
- private:
-  frc::Trajectory m_trajectory;
-  std::function<frc::Pose2d()> m_pose;
-  frc::RamseteController m_controller;
-  const units::volt_t m_ks;
-  const units::unit_t<voltsecondspermeter> m_kv;
-  const units::unit_t<voltsecondssquaredpermeter> m_ka;
-  frc::DifferentialDriveKinematics m_kinematics;
-  std::function<units::meters_per_second_t()> m_leftSpeed;
-  std::function<units::meters_per_second_t()> m_rightSpeed;
-  std::unique_ptr<frc2::PIDController> m_leftController;
-  std::unique_ptr<frc2::PIDController> m_rightController;
-  std::function<void(units::volt_t, units::volt_t)> m_outputVolts;
-  std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
-      m_outputVel;
-
-  Timer m_timer;
-  units::second_t m_prevTime;
-  frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/RunCommand.h b/wpilibc/src/main/native/include/frc2/command/RunCommand.h
deleted file mode 100644
index 5a0524a..0000000
--- a/wpilibc/src/main/native/include/frc2/command/RunCommand.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that runs a Runnable continuously.  Has no end condition as-is;
- * either subclass it or use Command.WithTimeout() or
- * Command.WithInterrupt() to give it one.  If you only wish
- * to execute a Runnable once, use InstantCommand.
- */
-class RunCommand : public CommandHelper<CommandBase, RunCommand> {
- public:
-  /**
-   * Creates a new RunCommand.  The Runnable will be run continuously until the
-   * command ends.  Does not run when disabled.
-   *
-   * @param toRun        the Runnable to run
-   * @param requirements the subsystems to require
-   */
-  RunCommand(std::function<void()> toRun,
-             std::initializer_list<Subsystem*> requirements);
-
-  RunCommand(RunCommand&& other) = default;
-
-  RunCommand(const RunCommand& other) = default;
-
-  void Execute();
-
- protected:
-  std::function<void()> m_toRun;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h b/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h
deleted file mode 100644
index 422823b..0000000
--- a/wpilibc/src/main/native/include/frc2/command/ScheduleCommand.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/SmallVector.h>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "SetUtilities.h"
-
-namespace frc2 {
-/**
- * Schedules the given commands when this command is initialized.  Useful for
- * forking off from CommandGroups.  Note that if run from a CommandGroup, the
- * group will not know about the status of the scheduled commands, and will
- * treat this command as finishing instantly.
- */
-class ScheduleCommand : public CommandHelper<CommandBase, ScheduleCommand> {
- public:
-  /**
-   * Creates a new ScheduleCommand that schedules the given commands when
-   * initialized.
-   *
-   * @param toSchedule the commands to schedule
-   */
-  explicit ScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
-
-  ScheduleCommand(ScheduleCommand&& other) = default;
-
-  ScheduleCommand(const ScheduleCommand& other) = default;
-
-  void Initialize() override;
-
-  bool IsFinished() override;
-
-  bool RunsWhenDisabled() const override;
-
- private:
-  wpi::SmallVector<Command*, 4> m_toSchedule;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/SelectCommand.h b/wpilibc/src/main/native/include/frc2/command/SelectCommand.h
deleted file mode 100644
index 8dba378..0000000
--- a/wpilibc/src/main/native/include/frc2/command/SelectCommand.h
+++ /dev/null
@@ -1,153 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <memory>
-#include <unordered_map>
-#include <utility>
-#include <vector>
-
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "PrintCommand.h"
-
-namespace frc2 {
-template <typename Key>
-/**
- * Runs one of a selection of commands, either using a selector and a key to
- * command mapping, or a supplier that returns the command directly at runtime.
- * Does not actually schedule the selected command - rather, the command is run
- * through this command; this ensures that the command will behave as expected
- * if used as part of a CommandGroup.  Requires the requirements of all included
- * commands, again to ensure proper functioning when used in a CommandGroup.  If
- * this is undesired, consider using ScheduleCommand.
- *
- * <p>As this command contains multiple component commands within it, it is
- * technically a command group; the command instances that are passed to it
- * cannot be added to any other groups, or scheduled individually.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class SelectCommand : public CommandHelper<CommandBase, SelectCommand<Key>> {
- public:
-  /**
-   * Creates a new selectcommand.
-   *
-   * @param commands the map of commands to choose from
-   * @param selector the selector to determine which command to run
-   */
-  template <class... Types,
-            typename = std::enable_if_t<std::conjunction_v<
-                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
-  SelectCommand(std::function<Key()> selector,
-                std::pair<Key, Types>... commands)
-      : m_selector{std::move(selector)} {
-    std::vector<std::pair<Key, std::unique_ptr<Command>>> foo;
-
-    ((void)foo.emplace_back(commands.first,
-                            std::make_unique<std::remove_reference_t<Types>>(
-                                std::move(commands.second))),
-     ...);
-
-    for (auto&& command : foo) {
-      if (!CommandGroupBase::RequireUngrouped(command.second)) {
-        return;
-      }
-    }
-
-    for (auto&& command : foo) {
-      this->AddRequirements(command.second->GetRequirements());
-      m_runsWhenDisabled &= command.second->RunsWhenDisabled();
-      m_commands.emplace(std::move(command.first), std::move(command.second));
-    }
-  }
-
-  SelectCommand(
-      std::function<Key()> selector,
-      std::vector<std::pair<Key, std::unique_ptr<Command>>>&& commands)
-      : m_selector{std::move(selector)} {
-    for (auto&& command : commands) {
-      if (!CommandGroupBase::RequireUngrouped(command.second)) {
-        return;
-      }
-    }
-
-    for (auto&& command : commands) {
-      this->AddRequirements(command.second->GetRequirements());
-      m_runsWhenDisabled &= command.second->RunsWhenDisabled();
-      m_commands.emplace(std::move(command.first), std::move(command.second));
-    }
-  }
-
-  // No copy constructors for command groups
-  SelectCommand(const SelectCommand& other) = delete;
-
-  // Prevent template expansion from emulating copy ctor
-  SelectCommand(SelectCommand&) = delete;
-
-  /**
-   * Creates a new selectcommand.
-   *
-   * @param toRun a supplier providing the command to run
-   */
-  explicit SelectCommand(std::function<Command*()> toRun) : m_toRun{toRun} {}
-
-  SelectCommand(SelectCommand&& other) = default;
-
-  void Initialize() override;
-
-  void Execute() override { m_selectedCommand->Execute(); }
-
-  void End(bool interrupted) override {
-    return m_selectedCommand->End(interrupted);
-  }
-
-  bool IsFinished() override { return m_selectedCommand->IsFinished(); }
-
-  bool RunsWhenDisabled() const override { return m_runsWhenDisabled; }
-
- protected:
-  std::unique_ptr<Command> TransferOwnership() && override {
-    return std::make_unique<SelectCommand>(std::move(*this));
-  }
-
- private:
-  std::unordered_map<Key, std::unique_ptr<Command>> m_commands;
-  std::function<Key()> m_selector;
-  std::function<Command*()> m_toRun;
-  Command* m_selectedCommand;
-  bool m_runsWhenDisabled = true;
-};
-
-template <typename T>
-void SelectCommand<T>::Initialize() {
-  if (m_selector) {
-    auto find = m_commands.find(m_selector());
-    if (find == m_commands.end()) {
-      m_selectedCommand = new PrintCommand(
-          "SelectCommand selector value does not correspond to any command!");
-      return;
-    }
-    m_selectedCommand = find->second.get();
-  } else {
-    m_selectedCommand = m_toRun();
-  }
-  m_selectedCommand->Initialize();
-}
-
-}  // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h b/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h
deleted file mode 100644
index dd1f2ce..0000000
--- a/wpilibc/src/main/native/include/frc2/command/SequentialCommandGroup.h
+++ /dev/null
@@ -1,104 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4521)
-#endif
-
-#include <limits>
-#include <memory>
-#include <utility>
-#include <vector>
-
-#include <wpi/ArrayRef.h>
-
-#include "CommandGroupBase.h"
-#include "CommandHelper.h"
-#include "frc/ErrorBase.h"
-#include "frc/WPIErrors.h"
-
-namespace frc2 {
-
-const size_t invalid_index = std::numeric_limits<size_t>::max();
-
-/**
- * A CommandGroups that runs a list of commands in sequence.
- *
- * <p>As a rule, CommandGroups require the union of the requirements of their
- * component commands.
- */
-class SequentialCommandGroup
-    : public CommandHelper<CommandGroupBase, SequentialCommandGroup> {
- public:
-  /**
-   * Creates a new SequentialCommandGroup.  The given commands will be run
-   * sequentially, with the CommandGroup finishing when the last command
-   * finishes.
-   *
-   * @param commands the commands to include in this group.
-   */
-  explicit SequentialCommandGroup(
-      std::vector<std::unique_ptr<Command>>&& commands);
-
-  /**
-   * Creates a new SequentialCommandGroup.  The given commands will be run
-   * sequentially, with the CommandGroup finishing when the last command
-   * finishes.
-   *
-   * @param commands the commands to include in this group.
-   */
-  template <class... Types,
-            typename = std::enable_if_t<std::conjunction_v<
-                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
-  explicit SequentialCommandGroup(Types&&... commands) {
-    AddCommands(std::forward<Types>(commands)...);
-  }
-
-  SequentialCommandGroup(SequentialCommandGroup&& other) = default;
-
-  // No copy constructors for command groups
-  SequentialCommandGroup(const SequentialCommandGroup&) = delete;
-
-  // Prevent template expansion from emulating copy ctor
-  SequentialCommandGroup(SequentialCommandGroup&) = delete;
-
-  template <class... Types,
-            typename = std::enable_if_t<std::conjunction_v<
-                std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
-  void AddCommands(Types&&... commands) {
-    std::vector<std::unique_ptr<Command>> foo;
-    ((void)foo.emplace_back(std::make_unique<std::remove_reference_t<Types>>(
-         std::forward<Types>(commands))),
-     ...);
-    AddCommands(std::move(foo));
-  }
-
-  void Initialize() override;
-
-  void Execute() override;
-
-  void End(bool interrupted) override;
-
-  bool IsFinished() override;
-
-  bool RunsWhenDisabled() const override;
-
- private:
-  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
-
-  wpi::SmallVector<std::unique_ptr<Command>, 4> m_commands;
-  size_t m_currentCommandIndex{invalid_index};
-  bool m_runWhenDisabled{true};
-};
-}  // namespace frc2
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
diff --git a/wpilibc/src/main/native/include/frc2/command/SetUtilities.h b/wpilibc/src/main/native/include/frc2/command/SetUtilities.h
deleted file mode 100644
index d21f572..0000000
--- a/wpilibc/src/main/native/include/frc2/command/SetUtilities.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/ArrayRef.h>
-#include <wpi/SmallVector.h>
-
-namespace frc2 {
-template <typename T>
-void SetInsert(wpi::SmallVectorImpl<T*>& vector, wpi::ArrayRef<T*> toAdd) {
-  for (auto addCommand : toAdd) {
-    bool exists = false;
-    for (auto existingCommand : vector) {
-      if (addCommand == existingCommand) {
-        exists = true;
-        break;
-      }
-    }
-    if (!exists) {
-      vector.emplace_back(addCommand);
-    }
-  }
-}
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h b/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h
deleted file mode 100644
index 987b9db..0000000
--- a/wpilibc/src/main/native/include/frc2/command/StartEndCommand.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that runs a given runnable when it is initalized, and another
- * runnable when it ends. Useful for running and then stopping a motor, or
- * extending and then retracting a solenoid. Has no end condition as-is; either
- * subclass it or use Command.WithTimeout() or Command.WithInterrupt() to give
- * it one.
- */
-class StartEndCommand : public CommandHelper<CommandBase, StartEndCommand> {
- public:
-  /**
-   * Creates a new StartEndCommand.  Will run the given runnables when the
-   * command starts and when it ends.
-   *
-   * @param onInit       the Runnable to run on command init
-   * @param onEnd        the Runnable to run on command end
-   * @param requirements the subsystems required by this command
-   */
-  StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
-                  std::initializer_list<Subsystem*> requirements);
-
-  StartEndCommand(StartEndCommand&& other) = default;
-
-  StartEndCommand(const StartEndCommand& other);
-
-  void Initialize() override;
-
-  void End(bool interrupted) override;
-
- protected:
-  std::function<void()> m_onInit;
-  std::function<void()> m_onEnd;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/Subsystem.h b/wpilibc/src/main/native/include/frc2/command/Subsystem.h
deleted file mode 100644
index 5eb10bc..0000000
--- a/wpilibc/src/main/native/include/frc2/command/Subsystem.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc2/command/CommandScheduler.h>
-
-#include <utility>
-
-namespace frc2 {
-class Command;
-/**
- * A robot subsystem.  Subsystems are the basic unit of robot organization in
- * the Command-based framework; they encapsulate low-level hardware objects
- * (motor controllers, sensors, etc) and provide methods through which they can
- * be used by Commands.  Subsystems are used by the CommandScheduler's resource
- * management system to ensure multiple robot actions are not "fighting" over
- * the same hardware; Commands that use a subsystem should include that
- * subsystem in their GetRequirements() method, and resources used within a
- * subsystem should generally remain encapsulated and not be shared by other
- * parts of the robot.
- *
- * <p>Subsystems must be registered with the scheduler with the
- * CommandScheduler.RegisterSubsystem() method in order for the
- * Periodic() method to be called.  It is recommended that this method be called
- * from the constructor of users' Subsystem implementations.  The
- * SubsystemBase class offers a simple base for user implementations
- * that handles this.
- *
- * @see Command
- * @see CommandScheduler
- * @see SubsystemBase
- */
-class Subsystem {
- public:
-  ~Subsystem();
-  /**
-   * This method is called periodically by the CommandScheduler.  Useful for
-   * updating subsystem-specific state that you don't want to offload to a
-   * Command.  Teams should try to be consistent within their own codebases
-   * about which responsibilities will be handled by Commands, and which will be
-   * handled here.
-   */
-  virtual void Periodic();
-
-  /**
-   * Sets the default Command of the subsystem.  The default command will be
-   * automatically scheduled when no other commands are scheduled that require
-   * the subsystem. Default commands should generally not end on their own, i.e.
-   * their IsFinished() method should always return false.  Will automatically
-   * register this subsystem with the CommandScheduler.
-   *
-   * @param defaultCommand the default command to associate with this subsystem
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  void SetDefaultCommand(T&& defaultCommand) {
-    CommandScheduler::GetInstance().SetDefaultCommand(
-        this, std::forward<T>(defaultCommand));
-  }
-
-  /**
-   * Gets the default command for this subsystem.  Returns null if no default
-   * command is currently associated with the subsystem.
-   *
-   * @return the default command associated with this subsystem
-   */
-  Command* GetDefaultCommand() const;
-
-  /**
-   * Returns the command currently running on this subsystem.  Returns null if
-   * no command is currently scheduled that requires this subsystem.
-   *
-   * @return the scheduled command currently requiring this subsystem
-   */
-  Command* GetCurrentCommand() const;
-
-  /**
-   * Registers this subsystem with the CommandScheduler, allowing its
-   * Periodic() method to be called when the scheduler runs.
-   */
-  void Register();
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h b/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h
deleted file mode 100644
index e7dffc7..0000000
--- a/wpilibc/src/main/native/include/frc2/command/SubsystemBase.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
-
-#include <string>
-
-#include "Subsystem.h"
-
-namespace frc2 {
-/**
- * A base for subsystems that handles registration in the constructor, and
- * provides a more intuitive method for setting the default command.
- */
-class SubsystemBase : public Subsystem,
-                      public frc::Sendable,
-                      public frc::SendableHelper<SubsystemBase> {
- public:
-  void InitSendable(frc::SendableBuilder& builder) override;
-
-  /**
-   * Gets the name of this Subsystem.
-   *
-   * @return Name
-   */
-  std::string GetName() const;
-
-  /**
-   * Sets the name of this Subsystem.
-   *
-   * @param name name
-   */
-  void SetName(const wpi::Twine& name);
-
-  /**
-   * Gets the subsystem name of this Subsystem.
-   *
-   * @return Subsystem name
-   */
-  std::string GetSubsystem() const;
-
-  /**
-   * Sets the subsystem name of this Subsystem.
-   *
-   * @param subsystem subsystem name
-   */
-  void SetSubsystem(const wpi::Twine& name);
-
-  /**
-   * Associate a Sendable with this Subsystem.
-   * Also update the child's name.
-   *
-   * @param name name to give child
-   * @param child sendable
-   */
-  void AddChild(std::string name, frc::Sendable* child);
-
- protected:
-  SubsystemBase();
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
deleted file mode 100644
index 6864f53..0000000
--- a/wpilibc/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <frc/trajectory/TrapezoidProfile.h>
-#include <frc2/Timer.h>
-
-#include <functional>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-
-#pragma once
-
-namespace frc2 {
-/**
- * A command that runs a TrapezoidProfile.  Useful for smoothly controlling
- * mechanism motion.
- *
- * @see TrapezoidProfile
- */
-class TrapezoidProfileCommand
-    : public CommandHelper<CommandBase, TrapezoidProfileCommand> {
- public:
-  /**
-   * Creates a new TrapezoidProfileCommand that will execute the given
-   * TrapezoidalProfile. Output will be piped to the provided consumer function.
-   *
-   * @param profile The motion profile to execute.
-   * @param output  The consumer for the profile output.
-   */
-  TrapezoidProfileCommand(
-      frc::TrapezoidProfile profile,
-      std::function<void(frc::TrapezoidProfile::State)> output,
-      std::initializer_list<Subsystem*> requirements);
-
-  void Initialize() override;
-
-  void Execute() override;
-
-  void End(bool interrupted) override;
-
-  bool IsFinished() override;
-
- private:
-  frc::TrapezoidProfile m_profile;
-  std::function<void(frc::TrapezoidProfile::State)> m_output;
-
-  Timer m_timer;
-};
-
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/WaitCommand.h b/wpilibc/src/main/native/include/frc2/command/WaitCommand.h
deleted file mode 100644
index 1544592..0000000
--- a/wpilibc/src/main/native/include/frc2/command/WaitCommand.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-#include <wpi/Twine.h>
-
-#include "CommandBase.h"
-#include "CommandHelper.h"
-#include "frc2/Timer.h"
-
-namespace frc2 {
-/**
- * A command that does nothing but takes a specified amount of time to finish.
- * Useful for CommandGroups.  Can also be subclassed to make a command with an
- * internal timer.
- */
-class WaitCommand : public CommandHelper<CommandBase, WaitCommand> {
- public:
-  /**
-   * Creates a new WaitCommand.  This command will do nothing, and end after the
-   * specified duration.
-   *
-   * @param duration the time to wait
-   */
-  explicit WaitCommand(units::second_t duration);
-
-  WaitCommand(WaitCommand&& other) = default;
-
-  WaitCommand(const WaitCommand& other) = default;
-
-  void Initialize() override;
-
-  void End(bool interrupted) override;
-
-  bool IsFinished() override;
-
-  bool RunsWhenDisabled() const override;
-
- protected:
-  Timer m_timer;
-
- private:
-  units::second_t m_duration;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h b/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h
deleted file mode 100644
index 8da18e8..0000000
--- a/wpilibc/src/main/native/include/frc2/command/WaitUntilCommand.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "CommandBase.h"
-#include "frc/Timer.h"
-#include "frc2/command/CommandHelper.h"
-
-namespace frc2 {
-/**
- * A command that does nothing but ends after a specified match time or
- * condition.  Useful for CommandGroups.
- */
-class WaitUntilCommand : public CommandHelper<CommandBase, WaitUntilCommand> {
- public:
-  /**
-   * Creates a new WaitUntilCommand that ends after a given condition becomes
-   * true.
-   *
-   * @param condition the condition to determine when to end
-   */
-  explicit WaitUntilCommand(std::function<bool()> condition);
-
-  /**
-   * Creates a new WaitUntilCommand that ends after a given match time.
-   *
-   * <p>NOTE: The match timer used for this command is UNOFFICIAL.  Using this
-   * command does NOT guarantee that the time at which the action is performed
-   * will be judged to be legal by the referees.  When in doubt, add a safety
-   * factor or time the action manually.
-   *
-   * @param time the match time after which to end, in seconds
-   */
-  explicit WaitUntilCommand(double time);
-
-  WaitUntilCommand(WaitUntilCommand&& other) = default;
-
-  WaitUntilCommand(const WaitUntilCommand& other) = default;
-
-  bool IsFinished() override;
-
-  bool RunsWhenDisabled() const override;
-
- private:
-  std::function<bool()> m_condition;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/Button.h b/wpilibc/src/main/native/include/frc2/command/button/Button.h
deleted file mode 100644
index 3e0f7fe..0000000
--- a/wpilibc/src/main/native/include/frc2/command/button/Button.h
+++ /dev/null
@@ -1,207 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-#include <utility>
-
-#include "Trigger.h"
-
-namespace frc2 {
-class Command;
-/**
- * A class used to bind command scheduling to button presses.  Can be composed
- * with other buttons with the operators in Trigger.
- *
- * @see Trigger
- */
-class Button : public Trigger {
- public:
-  /**
-   * Create a new button that is pressed when the given condition is true.
-   *
-   * @param isActive Whether the button is pressed.
-   */
-  explicit Button(std::function<bool()> isPressed);
-
-  /**
-   * Create a new button that is pressed active (default constructor) - activity
-   *  can be further determined by subclass code.
-   */
-  Button() = default;
-
-  /**
-   * Binds a command to start when the button is pressed.  Takes a
-   * raw pointer, and so is non-owning; users are responsible for the lifespan
-   * of the command.
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  Button WhenPressed(Command* command, bool interruptible = true);
-
-  /**
-   * Binds a command to start when the button is pressed.  Transfers
-   * command ownership to the button scheduler, so the user does not have to
-   * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
-   * *copied.*
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  Button WhenPressed(T&& command, bool interruptible = true) {
-    WhenActive(std::forward<T>(command), interruptible);
-    return *this;
-  }
-
-  /**
-   * Binds a runnable to execute when the button is pressed.
-   *
-   * @param toRun the runnable to execute.
-   */
-  Button WhenPressed(std::function<void()> toRun);
-
-  /**
-   * Binds a command to be started repeatedly while the button is pressed, and
-   * cancelled when it is released.  Takes a raw pointer, and so is non-owning;
-   * users are responsible for the lifespan of the command.
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The button, for chained calls.
-   */
-  Button WhileHeld(Command* command, bool interruptible = true);
-
-  /**
-   * Binds a command to be started repeatedly while the button is pressed, and
-   * cancelled when it is released.  Transfers command ownership to the button
-   * scheduler, so the user does not have to worry about lifespan - rvalue refs
-   * will be *moved*, lvalue refs will be *copied.*
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The button, for chained calls.
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  Button WhileHeld(T&& command, bool interruptible = true) {
-    WhileActiveContinous(std::forward<T>(command), interruptible);
-    return *this;
-  }
-
-  /**
-   * Binds a runnable to execute repeatedly while the button is pressed.
-   *
-   * @param toRun the runnable to execute.
-   */
-  Button WhileHeld(std::function<void()> toRun);
-
-  /**
-   * Binds a command to be started when the button is pressed, and cancelled
-   * when it is released.  Takes a raw pointer, and so is non-owning; users are
-   * responsible for the lifespan of the command.
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The button, for chained calls.
-   */
-  Button WhenHeld(Command* command, bool interruptible = true);
-
-  /**
-   * Binds a command to be started when the button is pressed, and cancelled
-   * when it is released.  Transfers command ownership to the button scheduler,
-   * so the user does not have to worry about lifespan - rvalue refs will be
-   * *moved*, lvalue refs will be *copied.*
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The button, for chained calls.
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  Button WhenHeld(T&& command, bool interruptible = true) {
-    WhileActiveOnce(std::forward<T>(command), interruptible);
-    return *this;
-  }
-
-  /**
-   * Binds a command to start when the button is released.  Takes a
-   * raw pointer, and so is non-owning; users are responsible for the lifespan
-   * of the command.
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The button, for chained calls.
-   */
-  Button WhenReleased(Command* command, bool interruptible = true);
-
-  /**
-   * Binds a command to start when the button is pressed.  Transfers
-   * command ownership to the button scheduler, so the user does not have to
-   * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
-   * *copied.*
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The button, for chained calls.
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  Button WhenReleased(T&& command, bool interruptible = true) {
-    WhenInactive(std::forward<T>(command), interruptible);
-    return *this;
-  }
-
-  /**
-   * Binds a runnable to execute when the button is released.
-   *
-   * @param toRun the runnable to execute.
-   */
-  Button WhenReleased(std::function<void()> toRun);
-
-  /**
-   * Binds a command to start when the button is pressed, and be cancelled when
-   * it is pressed again.  Takes a raw pointer, and so is non-owning; users are
-   * responsible for the lifespan of the command.
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The button, for chained calls.
-   */
-  Button ToggleWhenPressed(Command* command, bool interruptible = true);
-
-  /**
-   * Binds a command to start when the button is pressed, and be cancelled when
-   * it is pessed again.  Transfers command ownership to the button scheduler,
-   * so the user does not have to worry about lifespan - rvalue refs will be
-   * *moved*, lvalue refs will be *copied.*
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The button, for chained calls.
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  Button ToggleWhenPressed(T&& command, bool interruptible = true) {
-    ToggleWhenActive(std::forward<T>(command), interruptible);
-    return *this;
-  }
-
-  /**
-   * Binds a command to be cancelled when the button is pressed.  Takes a
-   * raw pointer, and so is non-owning; users are responsible for the lifespan
-   *  and scheduling of the command.
-   *
-   * @param command The command to bind.
-   * @return The button, for chained calls.
-   */
-  Button CancelWhenPressed(Command* command);
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h b/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h
deleted file mode 100644
index a23738b..0000000
--- a/wpilibc/src/main/native/include/frc2/command/button/JoystickButton.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-#include <frc/GenericHID.h>
-
-#include "Button.h"
-
-namespace frc2 {
-/**
- * A class used to bind command scheduling to joystick button presses.  Can be
- * composed with other buttons with the operators in Trigger.
- *
- * @see Trigger
- */
-class JoystickButton : public Button {
- public:
-  /**
-   * Creates a JoystickButton that commands can be bound to.
-   *
-   * @param joystick The joystick on which the button is located.
-   * @param buttonNumber The number of the button on the joystic.
-   */
-  explicit JoystickButton(frc::GenericHID* joystick, int buttonNumber)
-      : m_joystick{joystick}, m_buttonNumber{buttonNumber} {}
-
-  bool Get() const override { return m_joystick->GetRawButton(m_buttonNumber); }
-
- private:
-  frc::GenericHID* m_joystick;
-  int m_buttonNumber;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/POVButton.h b/wpilibc/src/main/native/include/frc2/command/button/POVButton.h
deleted file mode 100644
index 758cab2..0000000
--- a/wpilibc/src/main/native/include/frc2/command/button/POVButton.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-#include <frc/GenericHID.h>
-
-#include "Button.h"
-
-namespace frc2 {
-/**
- * A class used to bind command scheduling to joystick POV presses.  Can be
- * composed with other buttons with the operators in Trigger.
- *
- * @see Trigger
- */
-class POVButton : public Button {
- public:
-  /**
-   * Creates a POVButton that commands can be bound to.
-   *
-   * @param joystick The joystick on which the button is located.
-   * @param angle The angle of the POV corresponding to a button press.
-   * @param povNumber The number of the POV on the joystick.
-   */
-  POVButton(frc::GenericHID* joystick, int angle, int povNumber = 0)
-      : m_joystick{joystick}, m_angle{angle}, m_povNumber{povNumber} {}
-
-  bool Get() const override {
-    return m_joystick->GetPOV(m_povNumber) == m_angle;
-  }
-
- private:
-  frc::GenericHID* m_joystick;
-  int m_angle;
-  int m_povNumber;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc2/command/button/Trigger.h b/wpilibc/src/main/native/include/frc2/command/button/Trigger.h
deleted file mode 100644
index 1a65ff6..0000000
--- a/wpilibc/src/main/native/include/frc2/command/button/Trigger.h
+++ /dev/null
@@ -1,325 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc2/command/Command.h>
-#include <frc2/command/CommandScheduler.h>
-
-#include <atomic>
-#include <memory>
-#include <utility>
-
-namespace frc2 {
-class Command;
-/**
- * A class used to bind command scheduling to events.  The
- * Trigger class is a base for all command-event-binding classes, and so the
- * methods are named fairly abstractly; for purpose-specific wrappers, see
- * Button.
- *
- * @see Button
- */
-class Trigger {
- public:
-  /**
-   * Create a new trigger that is active when the given condition is true.
-   *
-   * @param isActive Whether the trigger is active.
-   */
-  explicit Trigger(std::function<bool()> isActive)
-      : m_isActive{std::move(isActive)} {}
-
-  /**
-   * Create a new trigger that is never active (default constructor) - activity
-   *  can be further determined by subclass code.
-   */
-  Trigger() {
-    m_isActive = [] { return false; };
-  }
-
-  Trigger(const Trigger& other);
-
-  /**
-   * Returns whether the trigger is active.  Can be overridden by a subclass.
-   *
-   * @return Whether the trigger is active.
-   */
-  virtual bool Get() const { return m_isActive(); }
-
-  /**
-   * Binds a command to start when the trigger becomes active.  Takes a
-   * raw pointer, and so is non-owning; users are responsible for the lifespan
-   * of the command.
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  Trigger WhenActive(Command* command, bool interruptible = true);
-
-  /**
-   * Binds a command to start when the trigger becomes active.  Transfers
-   * command ownership to the button scheduler, so the user does not have to
-   * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
-   * *copied.*
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  Trigger WhenActive(T&& command, bool interruptible = true) {
-    CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
-         command = std::make_unique<std::remove_reference_t<T>>(
-             std::forward<T>(command)),
-         interruptible]() mutable {
-          bool pressed = Get();
-
-          if (!pressedLast && pressed) {
-            command->Schedule(interruptible);
-          }
-
-          pressedLast = pressed;
-        });
-
-    return *this;
-  }
-
-  /**
-   * Binds a runnable to execute when the trigger becomes active.
-   *
-   * @param toRun the runnable to execute.
-   */
-  Trigger WhenActive(std::function<void()> toRun);
-
-  /**
-   * Binds a command to be started repeatedly while the trigger is active, and
-   * cancelled when it becomes inactive.  Takes a raw pointer, and so is
-   * non-owning; users are responsible for the lifespan of the command.
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  Trigger WhileActiveContinous(Command* command, bool interruptible = true);
-
-  /**
-   * Binds a command to be started repeatedly while the trigger is active, and
-   * cancelled when it becomes inactive.  Transfers command ownership to the
-   * button scheduler, so the user does not have to worry about lifespan -
-   * rvalue refs will be *moved*, lvalue refs will be *copied.*
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  Trigger WhileActiveContinous(T&& command, bool interruptible = true) {
-    CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
-         command = std::make_unique<std::remove_reference_t<T>>(
-             std::forward<T>(command)),
-         interruptible]() mutable {
-          bool pressed = Get();
-
-          if (pressed) {
-            command->Schedule(interruptible);
-          } else if (pressedLast && !pressed) {
-            command->Cancel();
-          }
-
-          pressedLast = pressed;
-        });
-    return *this;
-  }
-
-  /**
-   * Binds a runnable to execute repeatedly while the trigger is active.
-   *
-   * @param toRun the runnable to execute.
-   */
-  Trigger WhileActiveContinous(std::function<void()> toRun);
-
-  /**
-   * Binds a command to be started when the trigger becomes active, and
-   * cancelled when it becomes inactive.  Takes a raw pointer, and so is
-   * non-owning; users are responsible for the lifespan of the command.
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  Trigger WhileActiveOnce(Command* command, bool interruptible = true);
-
-  /**
-   * Binds a command to be started when the trigger becomes active, and
-   * cancelled when it becomes inactive.  Transfers command ownership to the
-   * button scheduler, so the user does not have to worry about lifespan -
-   * rvalue refs will be *moved*, lvalue refs will be *copied.*
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  Trigger WhileActiveOnce(T&& command, bool interruptible = true) {
-    CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
-         command = std::make_unique<std::remove_reference_t<T>>(
-             std::forward<T>(command)),
-         interruptible]() mutable {
-          bool pressed = Get();
-
-          if (!pressedLast && pressed) {
-            command->Schedule(interruptible);
-          } else if (pressedLast && !pressed) {
-            command->Cancel();
-          }
-
-          pressedLast = pressed;
-        });
-    return *this;
-  }
-
-  /**
-   * Binds a command to start when the trigger becomes inactive.  Takes a
-   * raw pointer, and so is non-owning; users are responsible for the lifespan
-   * of the command.
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  Trigger WhenInactive(Command* command, bool interruptible = true);
-
-  /**
-   * Binds a command to start when the trigger becomes inactive.  Transfers
-   * command ownership to the button scheduler, so the user does not have to
-   * worry about lifespan - rvalue refs will be *moved*, lvalue refs will be
-   * *copied.*
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  Trigger WhenInactive(T&& command, bool interruptible = true) {
-    CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
-         command = std::make_unique<std::remove_reference_t<T>>(
-             std::forward<T>(command)),
-         interruptible]() mutable {
-          bool pressed = Get();
-
-          if (pressedLast && !pressed) {
-            command->Schedule(interruptible);
-          }
-
-          pressedLast = pressed;
-        });
-    return *this;
-  }
-
-  /**
-   * Binds a runnable to execute when the trigger becomes inactive.
-   *
-   * @param toRun the runnable to execute.
-   */
-  Trigger WhenInactive(std::function<void()> toRun);
-
-  /**
-   * Binds a command to start when the trigger becomes active, and be cancelled
-   * when it again becomes active.  Takes a raw pointer, and so is non-owning;
-   * users are responsible for the lifespan of the command.
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  Trigger ToggleWhenActive(Command* command, bool interruptible = true);
-
-  /**
-   * Binds a command to start when the trigger becomes active, and be cancelled
-   * when it again becomes active.  Transfers command ownership to the button
-   * scheduler, so the user does not have to worry about lifespan - rvalue refs
-   * will be *moved*, lvalue refs will be *copied.*
-   *
-   * @param command The command to bind.
-   * @param interruptible Whether the command should be interruptible.
-   * @return The trigger, for chained calls.
-   */
-  template <class T, typename = std::enable_if_t<std::is_base_of_v<
-                         Command, std::remove_reference_t<T>>>>
-  Trigger ToggleWhenActive(T&& command, bool interruptible = true) {
-    CommandScheduler::GetInstance().AddButton(
-        [pressedLast = Get(), *this,
-         command = std::make_unique<std::remove_reference_t<T>>(
-             std::forward<T>(command)),
-         interruptible]() mutable {
-          bool pressed = Get();
-
-          if (!pressedLast && pressed) {
-            if (command->IsScheduled()) {
-              command->Cancel();
-            } else {
-              command->Schedule(interruptible);
-            }
-          }
-
-          pressedLast = pressed;
-        });
-    return *this;
-  }
-
-  /**
-   * Binds a command to be cancelled when the trigger becomes active.  Takes a
-   * raw pointer, and so is non-owning; users are responsible for the lifespan
-   *  and scheduling of the command.
-   *
-   * @param command The command to bind.
-   * @return The trigger, for chained calls.
-   */
-  Trigger CancelWhenActive(Command* command);
-
-  /**
-   * Composes two triggers with logical AND.
-   *
-   * @return A trigger which is active when both component triggers are active.
-   */
-  Trigger operator&&(Trigger rhs) {
-    return Trigger([*this, rhs] { return Get() && rhs.Get(); });
-  }
-
-  /**
-   * Composes two triggers with logical OR.
-   *
-   * @return A trigger which is active when either component trigger is active.
-   */
-  Trigger operator||(Trigger rhs) {
-    return Trigger([*this, rhs] { return Get() || rhs.Get(); });
-  }
-
-  /**
-   * Composes a trigger with logical NOT.
-   *
-   * @return A trigger which is active when the component trigger is inactive,
-   * and vice-versa.
-   */
-  Trigger operator!() {
-    return Trigger([*this] { return !Get(); });
-  }
-
- private:
-  std::function<bool()> m_isActive;
-};
-}  // namespace frc2
diff --git a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
index 888d183..0bb1002 100644
--- a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
+++ b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
@@ -45,20 +45,20 @@
 class LinearFilterNoiseTest
     : public testing::TestWithParam<LinearFilterNoiseTestType> {
  protected:
-  std::unique_ptr<frc::LinearFilter> m_filter;
+  std::unique_ptr<frc::LinearFilter<double>> m_filter;
 
   void SetUp() override {
     switch (GetParam()) {
       case TEST_SINGLE_POLE_IIR: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
-                                             kFilterStep));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+                                                     kFilterStep));
         break;
       }
 
       case TEST_MOVAVG: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::MovingAverage(kMovAvgTaps));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
         break;
       }
     }
diff --git a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
index 1f1476c..8db91b3 100644
--- a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
+++ b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
@@ -73,40 +73,41 @@
 class LinearFilterOutputTest
     : public testing::TestWithParam<LinearFilterOutputTestType> {
  protected:
-  std::unique_ptr<frc::LinearFilter> m_filter;
+  std::unique_ptr<frc::LinearFilter<double>> m_filter;
   std::function<double(double)> m_data;
   double m_expectedOutput = 0.0;
 
   void SetUp() override {
     switch (GetParam()) {
       case TEST_SINGLE_POLE_IIR: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::SinglePoleIIR(kSinglePoleIIRTimeConstant,
-                                             kFilterStep));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
+                                                     kFilterStep));
         m_data = GetData;
         m_expectedOutput = kSinglePoleIIRExpectedOutput;
         break;
       }
 
       case TEST_HIGH_PASS: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::HighPass(kHighPassTimeConstant, kFilterStep));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
+                                                kFilterStep));
         m_data = GetData;
         m_expectedOutput = kHighPassExpectedOutput;
         break;
       }
 
       case TEST_MOVAVG: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::MovingAverage(kMovAvgTaps));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
         m_data = GetData;
         m_expectedOutput = kMovAvgExpectedOutput;
         break;
       }
 
       case TEST_PULSE: {
-        m_filter = std::make_unique<frc::LinearFilter>(
-            frc::LinearFilter::MovingAverage(kMovAvgTaps));
+        m_filter = std::make_unique<frc::LinearFilter<double>>(
+            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
         m_data = GetPulseData;
         m_expectedOutput = 0.0;
         break;
diff --git a/wpilibc/src/test/native/cpp/MedianFilterTest.cpp b/wpilibc/src/test/native/cpp/MedianFilterTest.cpp
new file mode 100644
index 0000000..7fe7d2e
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/MedianFilterTest.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/MedianFilter.h"
+#include "gtest/gtest.h"
+
+TEST(MedianFilterTest, MedianFilterNotFullTestEven) {
+  frc::MedianFilter<double> filter{10};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(4);
+
+  EXPECT_EQ(filter.Calculate(1000), 3.5);
+}
+
+TEST(MedianFilterTest, MedianFilterNotFullTestOdd) {
+  frc::MedianFilter<double> filter{10};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(4);
+  filter.Calculate(7);
+
+  EXPECT_EQ(filter.Calculate(1000), 4);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestEven) {
+  frc::MedianFilter<double> filter{6};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(0);
+  filter.Calculate(5);
+  filter.Calculate(4);
+  filter.Calculate(1000);
+
+  EXPECT_EQ(filter.Calculate(99), 4.5);
+}
+
+TEST(MedianFilterTest, MedianFilterFullTestOdd) {
+  frc::MedianFilter<double> filter{5};
+
+  filter.Calculate(3);
+  filter.Calculate(0);
+  filter.Calculate(5);
+  filter.Calculate(4);
+  filter.Calculate(1000);
+
+  EXPECT_EQ(filter.Calculate(99), 5);
+}
diff --git a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
index 7bdac2e..c68cb37 100644
--- a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
+++ b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -27,7 +27,7 @@
 TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) {
   controller->SetP(4);
 
-  EXPECT_DOUBLE_EQ(-.1, controller->Calculate(.025, 0));
+  EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025, 0));
 }
 
 TEST_F(PIDInputOutputTest, IntegralGainOutputTest) {
@@ -36,10 +36,10 @@
   double out = 0;
 
   for (int i = 0; i < 5; i++) {
-    out = controller->Calculate(.025, 0);
+    out = controller->Calculate(0.025, 0);
   }
 
-  EXPECT_DOUBLE_EQ(-.5 * controller->GetPeriod().to<double>(), out);
+  EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
 }
 
 TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) {
@@ -47,6 +47,6 @@
 
   controller->Calculate(0, 0);
 
-  EXPECT_DOUBLE_EQ(-.01_s / controller->GetPeriod(),
-                   controller->Calculate(.0025, 0));
+  EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+                   controller->Calculate(0.0025, 0));
 }
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp
deleted file mode 100644
index 829f0d2..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/ButtonTest.cpp
+++ /dev/null
@@ -1,195 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/CommandScheduler.h"
-#include "frc2/command/RunCommand.h"
-#include "frc2/command/WaitUntilCommand.h"
-#include "frc2/command/button/Trigger.h"
-#include "gtest/gtest.h"
-
-using namespace frc2;
-class ButtonTest : public CommandTestBase {};
-
-TEST_F(ButtonTest, WhenPressedTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = false;
-
-  WaitUntilCommand command([&finished] { return finished; });
-
-  Trigger([&pressed] { return pressed; }).WhenActive(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  finished = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, WhenReleasedTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = false;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  pressed = true;
-  Trigger([&pressed] { return pressed; }).WhenInactive(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = false;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  finished = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, WhileHeldTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = false;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  pressed = false;
-  Trigger([&pressed] { return pressed; }).WhileActiveContinous(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  finished = true;
-  scheduler.Run();
-  finished = false;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  pressed = false;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, WhenHeldTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = false;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  pressed = false;
-  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  finished = true;
-  scheduler.Run();
-  finished = false;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-
-  pressed = false;
-  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
-  pressed = true;
-  scheduler.Run();
-  pressed = false;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, ToggleWhenPressedTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = false;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  pressed = false;
-  Trigger([&pressed] { return pressed; }).ToggleWhenActive(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  pressed = false;
-  scheduler.Run();
-  pressed = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, AndTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed1 = false;
-  bool pressed2 = false;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  (Trigger([&pressed1] { return pressed1; }) &&
-   Trigger([&pressed2] { return pressed2; }))
-      .WhenActive(&command);
-  pressed1 = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed2 = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, OrTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed1 = false;
-  bool pressed2 = false;
-  WaitUntilCommand command1([&finished] { return finished; });
-  WaitUntilCommand command2([&finished] { return finished; });
-
-  (Trigger([&pressed1] { return pressed1; }) ||
-   Trigger([&pressed2] { return pressed2; }))
-      .WhenActive(&command1);
-  pressed1 = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command1));
-
-  pressed1 = false;
-
-  (Trigger([&pressed1] { return pressed1; }) ||
-   Trigger([&pressed2] { return pressed2; }))
-      .WhenActive(&command2);
-  pressed2 = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command2));
-}
-
-TEST_F(ButtonTest, NegateTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = true;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  (!Trigger([&pressed] { return pressed; })).WhenActive(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = false;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, RValueButtonTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  int counter = 0;
-  bool pressed = false;
-
-  RunCommand command([&counter] { counter++; }, {});
-
-  Trigger([&pressed] { return pressed; }).WhenActive(std::move(command));
-  scheduler.Run();
-  EXPECT_EQ(counter, 0);
-  pressed = true;
-  scheduler.Run();
-  EXPECT_EQ(counter, 1);
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
deleted file mode 100644
index 0fe0b5b..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/ParallelRaceGroup.h"
-#include "frc2/command/PerpetualCommand.h"
-#include "frc2/command/RunCommand.h"
-#include "frc2/command/SequentialCommandGroup.h"
-
-using namespace frc2;
-class CommandDecoratorTest : public CommandTestBase {};
-
-TEST_F(CommandDecoratorTest, WithTimeoutTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  auto command = RunCommand([] {}, {}).WithTimeout(.1_s);
-
-  scheduler.Schedule(&command);
-
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-
-  std::this_thread::sleep_for(std::chrono::milliseconds(150));
-
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(CommandDecoratorTest, WithInterruptTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  bool finished = false;
-
-  auto command =
-      RunCommand([] {}, {}).WithInterrupt([&finished] { return finished; });
-
-  scheduler.Schedule(&command);
-
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-
-  finished = true;
-
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(CommandDecoratorTest, BeforeStartingTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  bool finished = false;
-
-  auto command = InstantCommand([] {}, {}).BeforeStarting(
-      [&finished] { finished = true; });
-
-  scheduler.Schedule(&command);
-
-  EXPECT_TRUE(finished);
-
-  scheduler.Run();
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(CommandDecoratorTest, AndThenTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  bool finished = false;
-
-  auto command =
-      InstantCommand([] {}, {}).AndThen([&finished] { finished = true; });
-
-  scheduler.Schedule(&command);
-
-  EXPECT_FALSE(finished);
-
-  scheduler.Run();
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  EXPECT_TRUE(finished);
-}
-
-TEST_F(CommandDecoratorTest, PerpetuallyTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  auto command = InstantCommand([] {}, {}).Perpetually();
-
-  scheduler.Schedule(&command);
-
-  scheduler.Run();
-  scheduler.Run();
-
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
deleted file mode 100644
index 2600430..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
+++ /dev/null
@@ -1,84 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/CommandScheduler.h"
-#include "frc2/command/ConditionalCommand.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/ParallelCommandGroup.h"
-#include "frc2/command/ParallelDeadlineGroup.h"
-#include "frc2/command/ParallelRaceGroup.h"
-#include "frc2/command/SelectCommand.h"
-#include "frc2/command/SequentialCommandGroup.h"
-
-using namespace frc2;
-class CommandRequirementsTest : public CommandTestBase {};
-
-TEST_F(CommandRequirementsTest, RequirementInterruptTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem requirement;
-
-  MockCommand command1({&requirement});
-  MockCommand command2({&requirement});
-
-  EXPECT_CALL(command1, Initialize());
-  EXPECT_CALL(command1, Execute());
-  EXPECT_CALL(command1, End(true));
-  EXPECT_CALL(command1, End(false)).Times(0);
-
-  EXPECT_CALL(command2, Initialize());
-  EXPECT_CALL(command2, Execute());
-  EXPECT_CALL(command2, End(true)).Times(0);
-  EXPECT_CALL(command2, End(false)).Times(0);
-
-  scheduler.Schedule(&command1);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command1));
-  scheduler.Schedule(&command2);
-  EXPECT_FALSE(scheduler.IsScheduled(&command1));
-  EXPECT_TRUE(scheduler.IsScheduled(&command2));
-  scheduler.Run();
-}
-
-TEST_F(CommandRequirementsTest, RequirementUninterruptibleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem requirement;
-
-  MockCommand command1({&requirement});
-  MockCommand command2({&requirement});
-
-  EXPECT_CALL(command1, Initialize());
-  EXPECT_CALL(command1, Execute()).Times(2);
-  EXPECT_CALL(command1, End(true)).Times(0);
-  EXPECT_CALL(command1, End(false)).Times(0);
-
-  EXPECT_CALL(command2, Initialize()).Times(0);
-  EXPECT_CALL(command2, Execute()).Times(0);
-  EXPECT_CALL(command2, End(true)).Times(0);
-  EXPECT_CALL(command2, End(false)).Times(0);
-
-  scheduler.Schedule(false, &command1);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command1));
-  scheduler.Schedule(&command2);
-  EXPECT_TRUE(scheduler.IsScheduled(&command1));
-  EXPECT_FALSE(scheduler.IsScheduled(&command2));
-  scheduler.Run();
-}
-
-TEST_F(CommandRequirementsTest, DefaultCommandRequirementErrorTest) {
-  TestSubsystem requirement1;
-  ErrorConfirmer confirmer("require");
-
-  MockCommand command1;
-
-  requirement1.SetDefaultCommand(std::move(command1));
-
-  EXPECT_TRUE(requirement1.GetDefaultCommand() == NULL);
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
deleted file mode 100644
index 6572a98..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-
-using namespace frc2;
-class CommandScheduleTest : public CommandTestBase {};
-
-TEST_F(CommandScheduleTest, InstantScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-  MockCommand command;
-
-  EXPECT_CALL(command, Initialize());
-  EXPECT_CALL(command, Execute());
-  EXPECT_CALL(command, End(false));
-
-  command.SetFinished(true);
-  scheduler.Schedule(&command);
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(CommandScheduleTest, SingleIterationScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-  MockCommand command;
-
-  EXPECT_CALL(command, Initialize());
-  EXPECT_CALL(command, Execute()).Times(2);
-  EXPECT_CALL(command, End(false));
-
-  scheduler.Schedule(&command);
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  scheduler.Run();
-  command.SetFinished(true);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(CommandScheduleTest, MultiScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-  MockCommand command1;
-  MockCommand command2;
-  MockCommand command3;
-
-  EXPECT_CALL(command1, Initialize());
-  EXPECT_CALL(command1, Execute()).Times(2);
-  EXPECT_CALL(command1, End(false));
-
-  EXPECT_CALL(command2, Initialize());
-  EXPECT_CALL(command2, Execute()).Times(3);
-  EXPECT_CALL(command2, End(false));
-
-  EXPECT_CALL(command3, Initialize());
-  EXPECT_CALL(command3, Execute()).Times(4);
-  EXPECT_CALL(command3, End(false));
-
-  scheduler.Schedule(&command1);
-  scheduler.Schedule(&command2);
-  scheduler.Schedule(&command3);
-  EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled({&command1, &command2, &command3}));
-  command1.SetFinished(true);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled({&command2, &command3}));
-  EXPECT_FALSE(scheduler.IsScheduled(&command1));
-  command2.SetFinished(true);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command3));
-  EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2}));
-  command3.SetFinished(true);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2, &command3}));
-}
-
-TEST_F(CommandScheduleTest, SchedulerCancelTest) {
-  CommandScheduler scheduler = GetScheduler();
-  MockCommand command;
-
-  EXPECT_CALL(command, Initialize());
-  EXPECT_CALL(command, Execute());
-  EXPECT_CALL(command, End(false)).Times(0);
-  EXPECT_CALL(command, End(true));
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  scheduler.Cancel(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(CommandScheduleTest, NotScheduledCancelTest) {
-  CommandScheduler scheduler = GetScheduler();
-  MockCommand command;
-
-  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&command));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp
deleted file mode 100644
index 0429c62..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-
-using namespace frc2;
-
-CommandTestBase::CommandTestBase() {
-  auto& scheduler = CommandScheduler::GetInstance();
-  scheduler.CancelAll();
-  scheduler.Enable();
-  scheduler.ClearButtons();
-}
-
-CommandScheduler CommandTestBase::GetScheduler() { return CommandScheduler(); }
-
-void CommandTestBase::SetUp() {
-  HALSIM_SetDriverStationEnabled(true);
-  while (!HALSIM_GetDriverStationEnabled()) {
-    std::this_thread::sleep_for(std::chrono::milliseconds(1));
-  }
-}
-
-void CommandTestBase::TearDown() {
-  CommandScheduler::GetInstance().ClearButtons();
-}
-
-void CommandTestBase::SetDSEnabled(bool enabled) {
-  HALSIM_SetDriverStationEnabled(enabled);
-  while (HALSIM_GetDriverStationEnabled() != static_cast<int>(enabled)) {
-    std::this_thread::sleep_for(std::chrono::milliseconds(1));
-  }
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h b/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h
deleted file mode 100644
index 8b22844..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/CommandTestBase.h
+++ /dev/null
@@ -1,102 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <regex>
-#include <utility>
-
-#include <mockdata/MockHooks.h>
-
-#include "ErrorConfirmer.h"
-#include "frc2/command/CommandGroupBase.h"
-#include "frc2/command/CommandScheduler.h"
-#include "frc2/command/SetUtilities.h"
-#include "frc2/command/SubsystemBase.h"
-#include "gmock/gmock.h"
-#include "gtest/gtest.h"
-#include "make_vector.h"
-#include "simulation/DriverStationSim.h"
-
-namespace frc2 {
-class CommandTestBase : public ::testing::Test {
- public:
-  CommandTestBase();
-
-  class TestSubsystem : public SubsystemBase {};
-
- protected:
-  class MockCommand : public Command {
-   public:
-    MOCK_CONST_METHOD0(GetRequirements, wpi::SmallSet<Subsystem*, 4>());
-    MOCK_METHOD0(IsFinished, bool());
-    MOCK_CONST_METHOD0(RunsWhenDisabled, bool());
-    MOCK_METHOD0(Initialize, void());
-    MOCK_METHOD0(Execute, void());
-    MOCK_METHOD1(End, void(bool interrupted));
-
-    MockCommand() {
-      m_requirements = {};
-      EXPECT_CALL(*this, GetRequirements())
-          .WillRepeatedly(::testing::Return(m_requirements));
-      EXPECT_CALL(*this, IsFinished()).WillRepeatedly(::testing::Return(false));
-      EXPECT_CALL(*this, RunsWhenDisabled())
-          .WillRepeatedly(::testing::Return(true));
-    }
-
-    MockCommand(std::initializer_list<Subsystem*> requirements,
-                bool finished = false, bool runWhenDisabled = true) {
-      m_requirements.insert(requirements.begin(), requirements.end());
-      EXPECT_CALL(*this, GetRequirements())
-          .WillRepeatedly(::testing::Return(m_requirements));
-      EXPECT_CALL(*this, IsFinished())
-          .WillRepeatedly(::testing::Return(finished));
-      EXPECT_CALL(*this, RunsWhenDisabled())
-          .WillRepeatedly(::testing::Return(runWhenDisabled));
-    }
-
-    MockCommand(MockCommand&& other) {
-      EXPECT_CALL(*this, IsFinished())
-          .WillRepeatedly(::testing::Return(other.IsFinished()));
-      EXPECT_CALL(*this, RunsWhenDisabled())
-          .WillRepeatedly(::testing::Return(other.RunsWhenDisabled()));
-      std::swap(m_requirements, other.m_requirements);
-      EXPECT_CALL(*this, GetRequirements())
-          .WillRepeatedly(::testing::Return(m_requirements));
-    }
-
-    MockCommand(const MockCommand& other) : Command{} {}
-
-    void SetFinished(bool finished) {
-      EXPECT_CALL(*this, IsFinished())
-          .WillRepeatedly(::testing::Return(finished));
-    }
-
-    ~MockCommand() {
-      auto& scheduler = CommandScheduler::GetInstance();
-      scheduler.Cancel(this);
-    }
-
-   protected:
-    std::unique_ptr<Command> TransferOwnership() && {
-      return std::make_unique<MockCommand>(std::move(*this));
-    }
-
-   private:
-    wpi::SmallSet<Subsystem*, 4> m_requirements;
-  };
-
-  CommandScheduler GetScheduler();
-
-  void SetUp() override;
-
-  void TearDown() override;
-
-  void SetDSEnabled(bool enabled);
-};
-}  // namespace frc2
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
deleted file mode 100644
index 927721a..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/ConditionalCommand.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/SelectCommand.h"
-
-using namespace frc2;
-class ConditionalCommandTest : public CommandTestBase {};
-
-TEST_F(ConditionalCommandTest, ConditionalCommandScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
-  MockCommand* mockptr = mock.get();
-
-  EXPECT_CALL(*mock, Initialize());
-  EXPECT_CALL(*mock, Execute()).Times(2);
-  EXPECT_CALL(*mock, End(false));
-
-  ConditionalCommand conditional(
-      std::move(mock), std::make_unique<InstantCommand>(), [] { return true; });
-
-  scheduler.Schedule(&conditional);
-  scheduler.Run();
-  mockptr->SetFinished(true);
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&conditional));
-}
-
-TEST_F(ConditionalCommandTest, ConditionalCommandRequirementTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem requirement1;
-  TestSubsystem requirement2;
-  TestSubsystem requirement3;
-  TestSubsystem requirement4;
-
-  InstantCommand command1([] {}, {&requirement1, &requirement2});
-  InstantCommand command2([] {}, {&requirement3});
-  InstantCommand command3([] {}, {&requirement3, &requirement4});
-
-  ConditionalCommand conditional(std::move(command1), std::move(command2),
-                                 [] { return true; });
-  scheduler.Schedule(&conditional);
-  scheduler.Schedule(&command3);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&command3));
-  EXPECT_FALSE(scheduler.IsScheduled(&conditional));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
deleted file mode 100644
index b97cbb6..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/RunCommand.h"
-
-using namespace frc2;
-class DefaultCommandTest : public CommandTestBase {};
-
-TEST_F(DefaultCommandTest, DefaultCommandScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem subsystem;
-
-  RunCommand command1([] {}, {&subsystem});
-
-  scheduler.SetDefaultCommand(&subsystem, std::move(command1));
-  auto handle = scheduler.GetDefaultCommand(&subsystem);
-  scheduler.Run();
-
-  EXPECT_TRUE(scheduler.IsScheduled(handle));
-}
-
-TEST_F(DefaultCommandTest, DefaultCommandInterruptResumeTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem subsystem;
-
-  RunCommand command1([] {}, {&subsystem});
-  RunCommand command2([] {}, {&subsystem});
-
-  scheduler.SetDefaultCommand(&subsystem, std::move(command1));
-  auto handle = scheduler.GetDefaultCommand(&subsystem);
-  scheduler.Run();
-  scheduler.Schedule(&command2);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&command2));
-  EXPECT_FALSE(scheduler.IsScheduled(handle));
-
-  scheduler.Cancel(&command2);
-  scheduler.Run();
-
-  EXPECT_TRUE(scheduler.IsScheduled(handle));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
deleted file mode 100644
index 2565a94..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "ErrorConfirmer.h"
-
-ErrorConfirmer* ErrorConfirmer::instance;
-
-int32_t ErrorConfirmer::HandleError(HAL_Bool isError, int32_t errorCode,
-                                    HAL_Bool isLVCode, const char* details,
-                                    const char* location, const char* callStack,
-                                    HAL_Bool printMsg) {
-  if (std::regex_search(details, std::regex(instance->m_msg))) {
-    instance->ConfirmError();
-  }
-  return 1;
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h b/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h
deleted file mode 100644
index 011158c..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/ErrorConfirmer.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <regex>
-
-#include <mockdata/MockHooks.h>
-
-#include "gmock/gmock.h"
-#include "simulation/DriverStationSim.h"
-
-class ErrorConfirmer {
- public:
-  explicit ErrorConfirmer(const char* msg) : m_msg(msg) {
-    if (instance != nullptr) return;
-    HALSIM_SetSendError(HandleError);
-    EXPECT_CALL(*this, ConfirmError());
-    instance = this;
-  }
-
-  ~ErrorConfirmer() {
-    HALSIM_SetSendError(nullptr);
-    instance = nullptr;
-  }
-
-  MOCK_METHOD0(ConfirmError, void());
-
-  const char* m_msg;
-
-  static int32_t HandleError(HAL_Bool isError, int32_t errorCode,
-                             HAL_Bool isLVCode, const char* details,
-                             const char* location, const char* callStack,
-                             HAL_Bool printMsg);
-
- private:
-  static ErrorConfirmer* instance;
-};
diff --git a/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
deleted file mode 100644
index 140d4fb..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
+++ /dev/null
@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/FunctionalCommand.h"
-
-using namespace frc2;
-class FunctionalCommandTest : public CommandTestBase {};
-
-TEST_F(FunctionalCommandTest, FunctionalCommandScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  int counter = 0;
-  bool finished = false;
-
-  FunctionalCommand command(
-      [&counter] { counter++; }, [&counter] { counter++; },
-      [&counter](bool) { counter++; }, [&finished] { return finished; });
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  finished = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  EXPECT_EQ(4, counter);
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
deleted file mode 100644
index e91f677..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/InstantCommand.h"
-
-using namespace frc2;
-class InstantCommandTest : public CommandTestBase {};
-
-TEST_F(InstantCommandTest, InstantCommandScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  int counter = 0;
-
-  InstantCommand command([&counter] { counter++; }, {});
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  EXPECT_EQ(counter, 1);
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
deleted file mode 100644
index 695ccc3..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/NotifierCommand.h"
-
-using namespace frc2;
-class NotifierCommandTest : public CommandTestBase {};
-
-#ifdef __APPLE__
-TEST_F(NotifierCommandTest, DISABLED_NotifierCommandScheduleTest) {
-#else
-TEST_F(NotifierCommandTest, NotifierCommandScheduleTest) {
-#endif
-  CommandScheduler scheduler = GetScheduler();
-
-  int counter = 0;
-
-  NotifierCommand command([&counter] { counter++; }, 0.01_s, {});
-
-  scheduler.Schedule(&command);
-  std::this_thread::sleep_for(std::chrono::milliseconds(250));
-  scheduler.Cancel(&command);
-
-  EXPECT_NEAR(.01 * counter, .25, .025);
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
deleted file mode 100644
index 55c7b98..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
+++ /dev/null
@@ -1,120 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/ParallelCommandGroup.h"
-#include "frc2/command/WaitUntilCommand.h"
-
-using namespace frc2;
-class ParallelCommandGroupTest : public CommandTestBase {};
-
-TEST_F(ParallelCommandGroupTest, ParallelGroupScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
-
-  MockCommand* command1 = command1Holder.get();
-  MockCommand* command2 = command2Holder.get();
-
-  ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
-      std::move(command1Holder), std::move(command2Holder)));
-
-  EXPECT_CALL(*command1, Initialize());
-  EXPECT_CALL(*command1, Execute()).Times(1);
-  EXPECT_CALL(*command1, End(false));
-
-  EXPECT_CALL(*command2, Initialize());
-  EXPECT_CALL(*command2, Execute()).Times(2);
-  EXPECT_CALL(*command2, End(false));
-
-  scheduler.Schedule(&group);
-
-  command1->SetFinished(true);
-  scheduler.Run();
-  command2->SetFinished(true);
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(ParallelCommandGroupTest, ParallelGroupInterruptTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
-
-  MockCommand* command1 = command1Holder.get();
-  MockCommand* command2 = command2Holder.get();
-
-  ParallelCommandGroup group(tcb::make_vector<std::unique_ptr<Command>>(
-      std::move(command1Holder), std::move(command2Holder)));
-
-  EXPECT_CALL(*command1, Initialize());
-  EXPECT_CALL(*command1, Execute()).Times(1);
-  EXPECT_CALL(*command1, End(false));
-
-  EXPECT_CALL(*command2, Initialize());
-  EXPECT_CALL(*command2, Execute()).Times(2);
-  EXPECT_CALL(*command2, End(false)).Times(0);
-  EXPECT_CALL(*command2, End(true));
-
-  scheduler.Schedule(&group);
-
-  command1->SetFinished(true);
-  scheduler.Run();
-  scheduler.Run();
-  scheduler.Cancel(&group);
-
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(ParallelCommandGroupTest, ParallelGroupNotScheduledCancelTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  ParallelCommandGroup group((InstantCommand(), InstantCommand()));
-
-  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
-}
-
-TEST_F(ParallelCommandGroupTest, ParallelGroupCopyTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  bool finished = false;
-
-  WaitUntilCommand command([&finished] { return finished; });
-
-  ParallelCommandGroup group(command);
-  scheduler.Schedule(&group);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&group));
-  finished = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(ParallelCommandGroupTest, ParallelGroupRequirementTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem requirement1;
-  TestSubsystem requirement2;
-  TestSubsystem requirement3;
-  TestSubsystem requirement4;
-
-  InstantCommand command1([] {}, {&requirement1, &requirement2});
-  InstantCommand command2([] {}, {&requirement3});
-  InstantCommand command3([] {}, {&requirement3, &requirement4});
-
-  ParallelCommandGroup group(std::move(command1), std::move(command2));
-
-  scheduler.Schedule(&group);
-  scheduler.Schedule(&command3);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&command3));
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
deleted file mode 100644
index 6e3246f..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
+++ /dev/null
@@ -1,136 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/ParallelDeadlineGroup.h"
-#include "frc2/command/WaitUntilCommand.h"
-
-using namespace frc2;
-class ParallelDeadlineGroupTest : public CommandTestBase {};
-
-TEST_F(ParallelDeadlineGroupTest, DeadlineGroupScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
-
-  MockCommand* command1 = command1Holder.get();
-  MockCommand* command2 = command2Holder.get();
-  MockCommand* command3 = command3Holder.get();
-
-  ParallelDeadlineGroup group(
-      std::move(command1Holder),
-      tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
-                                                 std::move(command3Holder)));
-
-  EXPECT_CALL(*command1, Initialize());
-  EXPECT_CALL(*command1, Execute()).Times(2);
-  EXPECT_CALL(*command1, End(false));
-
-  EXPECT_CALL(*command2, Initialize());
-  EXPECT_CALL(*command2, Execute()).Times(1);
-  EXPECT_CALL(*command2, End(false));
-
-  EXPECT_CALL(*command3, Initialize());
-  EXPECT_CALL(*command3, Execute()).Times(2);
-  EXPECT_CALL(*command3, End(true));
-
-  scheduler.Schedule(&group);
-
-  command2->SetFinished(true);
-  scheduler.Run();
-  command1->SetFinished(true);
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterruptTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem subsystem;
-
-  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
-
-  MockCommand* command1 = command1Holder.get();
-  MockCommand* command2 = command2Holder.get();
-  MockCommand* command3 = command3Holder.get();
-
-  ParallelDeadlineGroup group(
-      std::move(command1Holder),
-      tcb::make_vector<std::unique_ptr<Command>>(std::move(command2Holder),
-                                                 std::move(command3Holder)));
-
-  EXPECT_CALL(*command1, Initialize());
-  EXPECT_CALL(*command1, Execute()).Times(1);
-  EXPECT_CALL(*command1, End(true));
-
-  EXPECT_CALL(*command2, Initialize());
-  EXPECT_CALL(*command2, Execute()).Times(1);
-  EXPECT_CALL(*command2, End(true));
-
-  EXPECT_CALL(*command3, Initialize());
-  EXPECT_CALL(*command3, Execute()).Times(1);
-  EXPECT_CALL(*command3, End(true));
-
-  scheduler.Schedule(&group);
-
-  scheduler.Run();
-  scheduler.Cancel(&group);
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(ParallelDeadlineGroupTest, DeadlineGroupNotScheduledCancelTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  ParallelDeadlineGroup group{InstantCommand(), InstantCommand()};
-
-  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
-}
-
-TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopyTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  bool finished = false;
-
-  WaitUntilCommand command([&finished] { return finished; });
-
-  ParallelDeadlineGroup group(command);
-  scheduler.Schedule(&group);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&group));
-  finished = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirementTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem requirement1;
-  TestSubsystem requirement2;
-  TestSubsystem requirement3;
-  TestSubsystem requirement4;
-
-  InstantCommand command1([] {}, {&requirement1, &requirement2});
-  InstantCommand command2([] {}, {&requirement3});
-  InstantCommand command3([] {}, {&requirement3, &requirement4});
-
-  ParallelDeadlineGroup group(std::move(command1), std::move(command2));
-
-  scheduler.Schedule(&group);
-  scheduler.Schedule(&command3);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&command3));
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
deleted file mode 100644
index 54762ca..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
+++ /dev/null
@@ -1,156 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/ParallelRaceGroup.h"
-#include "frc2/command/SequentialCommandGroup.h"
-#include "frc2/command/WaitUntilCommand.h"
-
-using namespace frc2;
-class ParallelRaceGroupTest : public CommandTestBase {};
-
-TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
-
-  MockCommand* command1 = command1Holder.get();
-  MockCommand* command2 = command2Holder.get();
-  MockCommand* command3 = command3Holder.get();
-
-  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
-      std::move(command1Holder), std::move(command2Holder),
-      std::move(command3Holder))};
-
-  EXPECT_CALL(*command1, Initialize());
-  EXPECT_CALL(*command1, Execute()).Times(2);
-  EXPECT_CALL(*command1, End(true));
-
-  EXPECT_CALL(*command2, Initialize());
-  EXPECT_CALL(*command2, Execute()).Times(2);
-  EXPECT_CALL(*command2, End(false));
-
-  EXPECT_CALL(*command3, Initialize());
-  EXPECT_CALL(*command3, Execute()).Times(2);
-  EXPECT_CALL(*command3, End(true));
-
-  scheduler.Schedule(&group);
-
-  scheduler.Run();
-  command2->SetFinished(true);
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(ParallelRaceGroupTest, ParallelRaceInterruptTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
-
-  MockCommand* command1 = command1Holder.get();
-  MockCommand* command2 = command2Holder.get();
-  MockCommand* command3 = command3Holder.get();
-
-  ParallelRaceGroup group{tcb::make_vector<std::unique_ptr<Command>>(
-      std::move(command1Holder), std::move(command2Holder),
-      std::move(command3Holder))};
-
-  EXPECT_CALL(*command1, Initialize());
-  EXPECT_CALL(*command1, Execute()).Times(1);
-  EXPECT_CALL(*command1, End(true));
-
-  EXPECT_CALL(*command2, Initialize());
-  EXPECT_CALL(*command2, Execute()).Times(1);
-  EXPECT_CALL(*command2, End(true));
-
-  EXPECT_CALL(*command3, Initialize());
-  EXPECT_CALL(*command3, Execute()).Times(1);
-  EXPECT_CALL(*command3, End(true));
-
-  scheduler.Schedule(&group);
-
-  scheduler.Run();
-  scheduler.Cancel(&group);
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(ParallelRaceGroupTest, ParallelRaceNotScheduledCancelTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  ParallelRaceGroup group{InstantCommand(), InstantCommand()};
-
-  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
-}
-
-TEST_F(ParallelRaceGroupTest, ParallelRaceCopyTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  bool finished = false;
-
-  WaitUntilCommand command([&finished] { return finished; });
-
-  ParallelRaceGroup group(command);
-  scheduler.Schedule(&group);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&group));
-  finished = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(ParallelRaceGroupTest, RaceGroupRequirementTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem requirement1;
-  TestSubsystem requirement2;
-  TestSubsystem requirement3;
-  TestSubsystem requirement4;
-
-  InstantCommand command1([] {}, {&requirement1, &requirement2});
-  InstantCommand command2([] {}, {&requirement3});
-  InstantCommand command3([] {}, {&requirement3, &requirement4});
-
-  ParallelRaceGroup group(std::move(command1), std::move(command2));
-
-  scheduler.Schedule(&group);
-  scheduler.Schedule(&command3);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&command3));
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnceTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  bool finished1 = false;
-  bool finished2 = false;
-  bool finished3 = false;
-
-  WaitUntilCommand command1([&finished1] { return finished1; });
-  WaitUntilCommand command2([&finished2] { return finished2; });
-  WaitUntilCommand command3([&finished3] { return finished3; });
-
-  SequentialCommandGroup group1(command1, command2);
-  ParallelRaceGroup group2(std::move(group1), command3);
-
-  scheduler.Schedule(&group2);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&group2));
-  finished1 = true;
-  scheduler.Run();
-  finished2 = true;
-  EXPECT_NO_FATAL_FAILURE(scheduler.Run());
-  EXPECT_FALSE(scheduler.IsScheduled(&group2));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
deleted file mode 100644
index b3ef861..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/PerpetualCommand.h"
-
-using namespace frc2;
-class PerpetualCommandTest : public CommandTestBase {};
-
-TEST_F(PerpetualCommandTest, PerpetualCommandScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  bool check = false;
-
-  PerpetualCommand command{InstantCommand([&check] { check = true; }, {})};
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  EXPECT_TRUE(check);
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
deleted file mode 100644
index b940566..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <regex>
-
-#include "CommandTestBase.h"
-#include "frc2/command/PrintCommand.h"
-
-using namespace frc2;
-class PrintCommandTest : public CommandTestBase {};
-
-TEST_F(PrintCommandTest, PrintCommandScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  PrintCommand command("Test!");
-
-  testing::internal::CaptureStdout();
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-
-  EXPECT_TRUE(std::regex_search(testing::internal::GetCapturedStdout(),
-                                std::regex("Test!")));
-
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
deleted file mode 100644
index 09a569f..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <regex>
-
-#include "CommandTestBase.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/ProxyScheduleCommand.h"
-#include "frc2/command/WaitUntilCommand.h"
-
-using namespace frc2;
-class ProxyScheduleCommandTest : public CommandTestBase {};
-
-TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandScheduleTest) {
-  CommandScheduler& scheduler = CommandScheduler::GetInstance();
-
-  bool scheduled = false;
-
-  InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
-
-  ProxyScheduleCommand command(&toSchedule);
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-
-  EXPECT_TRUE(scheduled);
-}
-
-TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandEndTest) {
-  CommandScheduler& scheduler = CommandScheduler::GetInstance();
-
-  bool finished = false;
-
-  WaitUntilCommand toSchedule([&finished] { return finished; });
-
-  ProxyScheduleCommand command(&toSchedule);
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  finished = true;
-  scheduler.Run();
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
deleted file mode 100644
index bac40d5..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
+++ /dev/null
@@ -1,156 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/ConditionalCommand.h"
-#include "frc2/command/ParallelCommandGroup.h"
-#include "frc2/command/ParallelDeadlineGroup.h"
-#include "frc2/command/ParallelRaceGroup.h"
-#include "frc2/command/SelectCommand.h"
-#include "frc2/command/SequentialCommandGroup.h"
-
-using namespace frc2;
-class RobotDisabledCommandTest : public CommandTestBase {};
-
-TEST_F(RobotDisabledCommandTest, RobotDisabledCommandCancelTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  MockCommand command({}, false, false);
-
-  EXPECT_CALL(command, End(true));
-
-  SetDSEnabled(true);
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-
-  SetDSEnabled(false);
-
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(RobotDisabledCommandTest, RunWhenDisabledTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  MockCommand command1;
-  MockCommand command2;
-
-  scheduler.Schedule(&command1);
-
-  SetDSEnabled(false);
-
-  scheduler.Run();
-
-  scheduler.Schedule(&command2);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&command1));
-  EXPECT_TRUE(scheduler.IsScheduled(&command2));
-}
-
-TEST_F(RobotDisabledCommandTest, SequentialGroupRunWhenDisabledTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  SequentialCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
-  SequentialCommandGroup dontRunWhenDisabled{MockCommand(),
-                                             MockCommand({}, false, false)};
-
-  SetDSEnabled(false);
-
-  scheduler.Schedule(&runWhenDisabled);
-  scheduler.Schedule(&dontRunWhenDisabled);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
-  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
-}
-
-TEST_F(RobotDisabledCommandTest, ParallelGroupRunWhenDisabledTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  ParallelCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
-  ParallelCommandGroup dontRunWhenDisabled{MockCommand(),
-                                           MockCommand({}, false, false)};
-
-  SetDSEnabled(false);
-
-  scheduler.Schedule(&runWhenDisabled);
-  scheduler.Schedule(&dontRunWhenDisabled);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
-  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
-}
-
-TEST_F(RobotDisabledCommandTest, ParallelRaceRunWhenDisabledTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  ParallelRaceGroup runWhenDisabled{MockCommand(), MockCommand()};
-  ParallelRaceGroup dontRunWhenDisabled{MockCommand(),
-                                        MockCommand({}, false, false)};
-
-  SetDSEnabled(false);
-
-  scheduler.Schedule(&runWhenDisabled);
-  scheduler.Schedule(&dontRunWhenDisabled);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
-  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
-}
-
-TEST_F(RobotDisabledCommandTest, ParallelDeadlineRunWhenDisabledTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  ParallelDeadlineGroup runWhenDisabled{MockCommand(), MockCommand()};
-  ParallelDeadlineGroup dontRunWhenDisabled{MockCommand(),
-                                            MockCommand({}, false, false)};
-
-  SetDSEnabled(false);
-
-  scheduler.Schedule(&runWhenDisabled);
-  scheduler.Schedule(&dontRunWhenDisabled);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
-  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
-}
-
-TEST_F(RobotDisabledCommandTest, ConditionalCommandRunWhenDisabledTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  ConditionalCommand runWhenDisabled{MockCommand(), MockCommand(),
-                                     [] { return true; }};
-  ConditionalCommand dontRunWhenDisabled{
-      MockCommand(), MockCommand({}, false, false), [] { return true; }};
-
-  SetDSEnabled(false);
-
-  scheduler.Schedule(&runWhenDisabled);
-  scheduler.Schedule(&dontRunWhenDisabled);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
-  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
-}
-
-TEST_F(RobotDisabledCommandTest, SelectCommandRunWhenDisabledTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  SelectCommand<int> runWhenDisabled{[] { return 1; },
-                                     std::pair(1, MockCommand()),
-                                     std::pair(1, MockCommand())};
-  SelectCommand<int> dontRunWhenDisabled{
-      [] { return 1; }, std::pair(1, MockCommand()),
-      std::pair(1, MockCommand({}, false, false))};
-
-  SetDSEnabled(false);
-
-  scheduler.Schedule(&runWhenDisabled);
-  scheduler.Schedule(&dontRunWhenDisabled);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&runWhenDisabled));
-  EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp
deleted file mode 100644
index 07eecb3..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/RunCommandTest.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/RunCommand.h"
-
-using namespace frc2;
-class RunCommandTest : public CommandTestBase {};
-
-TEST_F(RunCommandTest, RunCommandScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  int counter = 0;
-
-  RunCommand command([&counter] { counter++; }, {});
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-  scheduler.Run();
-  scheduler.Run();
-
-  EXPECT_EQ(3, counter);
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
deleted file mode 100644
index 29e8dc5..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <regex>
-
-#include "CommandTestBase.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/ScheduleCommand.h"
-#include "frc2/command/SequentialCommandGroup.h"
-
-using namespace frc2;
-class ScheduleCommandTest : public CommandTestBase {};
-
-TEST_F(ScheduleCommandTest, ScheduleCommandScheduleTest) {
-  CommandScheduler& scheduler = CommandScheduler::GetInstance();
-
-  bool scheduled = false;
-
-  InstantCommand toSchedule([&scheduled] { scheduled = true; }, {});
-
-  ScheduleCommand command(&toSchedule);
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-
-  EXPECT_TRUE(scheduled);
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp
deleted file mode 100644
index f0198c9..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/SchedulerTest.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/RunCommand.h"
-
-using namespace frc2;
-class SchedulerTest : public CommandTestBase {};
-
-TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) {
-  CommandScheduler scheduler = GetScheduler();
-
-  InstantCommand command;
-
-  int counter = 0;
-
-  scheduler.OnCommandInitialize([&counter](const Command&) { counter++; });
-  scheduler.OnCommandExecute([&counter](const Command&) { counter++; });
-  scheduler.OnCommandFinish([&counter](const Command&) { counter++; });
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-
-  EXPECT_EQ(counter, 3);
-}
-
-TEST_F(SchedulerTest, SchedulerLambdaInterruptTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  RunCommand command([] {}, {});
-
-  int counter = 0;
-
-  scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; });
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-  scheduler.Cancel(&command);
-
-  EXPECT_EQ(counter, 1);
-}
-
-TEST_F(SchedulerTest, UnregisterSubsystemTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem system;
-
-  scheduler.RegisterSubsystem(&system);
-
-  EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
deleted file mode 100644
index 6be14ef..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
+++ /dev/null
@@ -1,62 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/ConditionalCommand.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/SelectCommand.h"
-
-using namespace frc2;
-class SelectCommandTest : public CommandTestBase {};
-
-TEST_F(SelectCommandTest, SelectCommandTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
-  MockCommand* mockptr = mock.get();
-
-  EXPECT_CALL(*mock, Initialize());
-  EXPECT_CALL(*mock, Execute()).Times(2);
-  EXPECT_CALL(*mock, End(false));
-
-  std::vector<std::pair<int, std::unique_ptr<Command>>> temp;
-
-  temp.emplace_back(std::pair(1, std::move(mock)));
-  temp.emplace_back(std::pair(2, std::make_unique<InstantCommand>()));
-  temp.emplace_back(std::pair(3, std::make_unique<InstantCommand>()));
-
-  SelectCommand<int> select([] { return 1; }, std::move(temp));
-
-  scheduler.Schedule(&select);
-  scheduler.Run();
-  mockptr->SetFinished(true);
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&select));
-}
-
-TEST_F(SelectCommandTest, SelectCommandRequirementTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem requirement1;
-  TestSubsystem requirement2;
-  TestSubsystem requirement3;
-  TestSubsystem requirement4;
-
-  InstantCommand command1([] {}, {&requirement1, &requirement2});
-  InstantCommand command2([] {}, {&requirement3});
-  InstantCommand command3([] {}, {&requirement3, &requirement4});
-
-  SelectCommand<int> select([] { return 1; }, std::pair(1, std::move(command1)),
-                            std::pair(2, std::move(command2)));
-
-  scheduler.Schedule(&select);
-  scheduler.Schedule(&command3);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&command3));
-  EXPECT_FALSE(scheduler.IsScheduled(&select));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
deleted file mode 100644
index 3a6f8d8..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
+++ /dev/null
@@ -1,137 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/InstantCommand.h"
-#include "frc2/command/SequentialCommandGroup.h"
-#include "frc2/command/WaitUntilCommand.h"
-
-using namespace frc2;
-class SequentialCommandGroupTest : public CommandTestBase {};
-
-TEST_F(SequentialCommandGroupTest, SequentialGroupScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
-
-  MockCommand* command1 = command1Holder.get();
-  MockCommand* command2 = command2Holder.get();
-  MockCommand* command3 = command3Holder.get();
-
-  SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
-      std::move(command1Holder), std::move(command2Holder),
-      std::move(command3Holder))};
-
-  EXPECT_CALL(*command1, Initialize());
-  EXPECT_CALL(*command1, Execute()).Times(1);
-  EXPECT_CALL(*command1, End(false));
-
-  EXPECT_CALL(*command2, Initialize());
-  EXPECT_CALL(*command2, Execute()).Times(1);
-  EXPECT_CALL(*command2, End(false));
-
-  EXPECT_CALL(*command3, Initialize());
-  EXPECT_CALL(*command3, Execute()).Times(1);
-  EXPECT_CALL(*command3, End(false));
-
-  scheduler.Schedule(&group);
-
-  command1->SetFinished(true);
-  scheduler.Run();
-  command2->SetFinished(true);
-  scheduler.Run();
-  command3->SetFinished(true);
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(SequentialCommandGroupTest, SequentialGroupInterruptTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command2Holder = std::make_unique<MockCommand>();
-  std::unique_ptr<MockCommand> command3Holder = std::make_unique<MockCommand>();
-
-  MockCommand* command1 = command1Holder.get();
-  MockCommand* command2 = command2Holder.get();
-  MockCommand* command3 = command3Holder.get();
-
-  SequentialCommandGroup group{tcb::make_vector<std::unique_ptr<Command>>(
-      std::move(command1Holder), std::move(command2Holder),
-      std::move(command3Holder))};
-
-  EXPECT_CALL(*command1, Initialize());
-  EXPECT_CALL(*command1, Execute()).Times(1);
-  EXPECT_CALL(*command1, End(false));
-
-  EXPECT_CALL(*command2, Initialize());
-  EXPECT_CALL(*command2, Execute()).Times(0);
-  EXPECT_CALL(*command2, End(false)).Times(0);
-  EXPECT_CALL(*command2, End(true));
-
-  EXPECT_CALL(*command3, Initialize()).Times(0);
-  EXPECT_CALL(*command3, Execute()).Times(0);
-  EXPECT_CALL(*command3, End(false)).Times(0);
-  EXPECT_CALL(*command3, End(true)).Times(0);
-
-  scheduler.Schedule(&group);
-
-  command1->SetFinished(true);
-  scheduler.Run();
-  scheduler.Cancel(&group);
-  scheduler.Run();
-
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(SequentialCommandGroupTest, SequentialGroupNotScheduledCancelTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  SequentialCommandGroup group{InstantCommand(), InstantCommand()};
-
-  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
-}
-
-TEST_F(SequentialCommandGroupTest, SequentialGroupCopyTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  bool finished = false;
-
-  WaitUntilCommand command([&finished] { return finished; });
-
-  SequentialCommandGroup group(command);
-  scheduler.Schedule(&group);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&group));
-  finished = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
-
-TEST_F(SequentialCommandGroupTest, SequentialGroupRequirementTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  TestSubsystem requirement1;
-  TestSubsystem requirement2;
-  TestSubsystem requirement3;
-  TestSubsystem requirement4;
-
-  InstantCommand command1([] {}, {&requirement1, &requirement2});
-  InstantCommand command2([] {}, {&requirement3});
-  InstantCommand command3([] {}, {&requirement3, &requirement4});
-
-  SequentialCommandGroup group(std::move(command1), std::move(command2));
-
-  scheduler.Schedule(&group);
-  scheduler.Schedule(&command3);
-
-  EXPECT_TRUE(scheduler.IsScheduled(&command3));
-  EXPECT_FALSE(scheduler.IsScheduled(&group));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
deleted file mode 100644
index 3f25792..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/StartEndCommand.h"
-
-using namespace frc2;
-class StartEndCommandTest : public CommandTestBase {};
-
-TEST_F(StartEndCommandTest, StartEndCommandScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  int counter = 0;
-
-  StartEndCommand command([&counter] { counter++; }, [&counter] { counter++; },
-                          {});
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-  scheduler.Run();
-  scheduler.Cancel(&command);
-
-  EXPECT_EQ(2, counter);
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
deleted file mode 100644
index 3363975..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/WaitCommand.h"
-#include "frc2/command/WaitUntilCommand.h"
-
-using namespace frc2;
-class WaitCommandTest : public CommandTestBase {};
-
-TEST_F(WaitCommandTest, WaitCommandScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  WaitCommand command(.1_s);
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  std::this_thread::sleep_for(std::chrono::milliseconds(110));
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp b/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
deleted file mode 100644
index e9728db..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "CommandTestBase.h"
-#include "frc2/command/WaitUntilCommand.h"
-
-using namespace frc2;
-class WaitUntilCommandTest : public CommandTestBase {};
-
-TEST_F(WaitUntilCommandTest, WaitUntilCommandScheduleTest) {
-  CommandScheduler scheduler = GetScheduler();
-
-  bool finished = false;
-
-  WaitUntilCommand command([&finished] { return finished; });
-
-  scheduler.Schedule(&command);
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  finished = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
diff --git a/wpilibc/src/test/native/cpp/frc2/command/make_vector.h b/wpilibc/src/test/native/cpp/frc2/command/make_vector.h
deleted file mode 100644
index 89d55b6..0000000
--- a/wpilibc/src/test/native/cpp/frc2/command/make_vector.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <type_traits>
-#include <utility>
-#include <vector>
-
-namespace tcb {
-
-namespace detail {
-
-template <typename T, typename...>
-struct vec_type_helper {
-  using type = T;
-};
-
-template <typename... Args>
-struct vec_type_helper<void, Args...> {
-  using type = typename std::common_type_t<Args...>;
-};
-
-template <typename T, typename... Args>
-using vec_type_helper_t = typename vec_type_helper<T, Args...>::type;
-
-template <typename, typename...>
-struct all_constructible_and_convertible : std::true_type {};
-
-template <typename T, typename First, typename... Rest>
-struct all_constructible_and_convertible<T, First, Rest...>
-    : std::conditional_t<
-          std::is_constructible_v<T, First> && std::is_convertible_v<First, T>,
-          all_constructible_and_convertible<T, Rest...>, std::false_type> {};
-
-template <typename T, typename... Args,
-          typename std::enable_if_t<!std::is_trivially_copyable_v<T>, int> = 0>
-std::vector<T> make_vector_impl(Args&&... args) {
-  std::vector<T> vec;
-  vec.reserve(sizeof...(Args));
-  using arr_t = int[];
-  (void)arr_t{0, (vec.emplace_back(std::forward<Args>(args)), 0)...};
-  return vec;
-}
-
-template <typename T, typename... Args,
-          typename std::enable_if_t<std::is_trivially_copyable_v<T>, int> = 0>
-std::vector<T> make_vector_impl(Args&&... args) {
-  return std::vector<T>{std::forward<Args>(args)...};
-}
-
-}  // namespace detail
-
-template <
-    typename T = void, typename... Args,
-    typename V = detail::vec_type_helper_t<T, Args...>,
-    typename std::enable_if_t<
-        detail::all_constructible_and_convertible<V, Args...>::value, int> = 0>
-std::vector<V> make_vector(Args&&... args) {
-  return detail::make_vector_impl<V>(std::forward<Args>(args)...);
-}
-
-}  // namespace tcb
diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
index 2647252..8acaf92 100644
--- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -15,30 +15,11 @@
 
 using namespace frc;
 
-TEST(DifferentialDriveOdometry, OneIteration) {
-  DifferentialDriveKinematics kinematics{0.381_m * 2};
-  DifferentialDriveOdometry odometry{kinematics};
+TEST(DifferentialDriveOdometry, EncoderDistances) {
+  DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
 
-  odometry.ResetPosition(Pose2d());
-  DifferentialDriveWheelSpeeds wheelSpeeds{0.02_mps, 0.02_mps};
-  odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
-  const auto& pose = odometry.UpdateWithTime(1_s, Rotation2d(), wheelSpeeds);
-
-  EXPECT_NEAR(pose.Translation().X().to<double>(), 0.02, kEpsilon);
-  EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
-}
-
-TEST(DifferentialDriveOdometry, QuarterCircle) {
-  DifferentialDriveKinematics kinematics{0.381_m * 2};
-  DifferentialDriveOdometry odometry{kinematics};
-
-  odometry.ResetPosition(Pose2d());
-  DifferentialDriveWheelSpeeds wheelSpeeds{
-      0.0_mps, units::meters_per_second_t(5 * wpi::math::pi)};
-  odometry.UpdateWithTime(0_s, Rotation2d(), DifferentialDriveWheelSpeeds());
-  const auto& pose =
-      odometry.UpdateWithTime(1_s, Rotation2d(90_deg), wheelSpeeds);
+  const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
+                                     units::meter_t(5 * wpi::math::pi));
 
   EXPECT_NEAR(pose.Translation().X().to<double>(), 5.0, kEpsilon);
   EXPECT_NEAR(pose.Translation().Y().to<double>(), 5.0, kEpsilon);
diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
index 0a6894d..5d990bf 100644
--- a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -18,11 +18,11 @@
   Translation2d m_br{-12_m, -12_m};
 
   MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
-  MecanumDriveOdometry odometry{kinematics};
+  MecanumDriveOdometry odometry{kinematics, 0_rad};
 };
 
 TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
-  odometry.ResetPosition(Pose2d());
+  odometry.ResetPosition(Pose2d(), 0_rad);
   MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
                                       3.536_mps};
 
@@ -35,7 +35,7 @@
 }
 
 TEST_F(MecanumDriveOdometryTest, TwoIterations) {
-  odometry.ResetPosition(Pose2d());
+  odometry.ResetPosition(Pose2d(), 0_rad);
   MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
 
   odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
@@ -47,7 +47,7 @@
 }
 
 TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
-  odometry.ResetPosition(Pose2d());
+  odometry.ResetPosition(Pose2d(), 0_rad);
   MecanumDriveWheelSpeeds speeds{-13.328_mps, 39.986_mps, -13.329_mps,
                                  39.986_mps};
   odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
@@ -57,3 +57,16 @@
   EXPECT_NEAR(pose.Translation().Y().to<double>(), 12, 0.01);
   EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
 }
+
+TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
+  odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+
+  MecanumDriveWheelSpeeds speeds{3.536_mps, 3.536_mps, 3.536_mps, 3.536_mps};
+
+  odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
+  auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
+
+  EXPECT_NEAR(pose.Translation().X().to<double>(), 0.5, 0.01);
+  EXPECT_NEAR(pose.Translation().Y().to<double>(), 0.0, 0.01);
+  EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+}
diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
index ab81017..d0399dc 100644
--- a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -21,13 +21,13 @@
   Translation2d m_br{-12_m, -12_m};
 
   SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
-  SwerveDriveOdometry<4> m_odometry{m_kinematics};
+  SwerveDriveOdometry<4> m_odometry{m_kinematics, 0_rad};
 };
 
 TEST_F(SwerveDriveOdometryTest, TwoIterations) {
   SwerveModuleState state{5_mps, Rotation2d()};
 
-  m_odometry.ResetPosition(Pose2d());
+  m_odometry.ResetPosition(Pose2d(), 0_rad);
   m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
                             SwerveModuleState(), SwerveModuleState(),
                             SwerveModuleState());
@@ -47,7 +47,7 @@
 
   SwerveModuleState zero{0_mps, Rotation2d()};
 
-  m_odometry.ResetPosition(Pose2d());
+  m_odometry.ResetPosition(Pose2d(), 0_rad);
   m_odometry.UpdateWithTime(0_s, Rotation2d(), zero, zero, zero, zero);
   auto pose =
       m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
@@ -56,3 +56,19 @@
   EXPECT_NEAR(12.0, pose.Translation().Y().to<double>(), kEpsilon);
   EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
 }
+
+TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
+  m_odometry.ResetPosition(Pose2d(), Rotation2d(90_deg));
+
+  SwerveModuleState state{5_mps, Rotation2d()};
+
+  m_odometry.UpdateWithTime(0_s, Rotation2d(90_deg), SwerveModuleState(),
+                            SwerveModuleState(), SwerveModuleState(),
+                            SwerveModuleState());
+  auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
+                                        state, state);
+
+  EXPECT_NEAR(0.5, pose.Translation().X().to<double>(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Translation().Y().to<double>(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+}
diff --git a/wpilibc/src/test/native/cpp/main.cpp b/wpilibc/src/test/native/cpp/main.cpp
index 0e00efa..c6b6c58 100644
--- a/wpilibc/src/test/native/cpp/main.cpp
+++ b/wpilibc/src/test/native/cpp/main.cpp
@@ -1,11 +1,11 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#include <hal/HAL.h>
+#include <hal/HALBase.h>
 
 #include "gtest/gtest.h"
 
diff --git a/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp b/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
deleted file mode 100644
index 23f3e3a..0000000
--- a/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
+++ /dev/null
@@ -1,115 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 <array>
-#include <memory>
-#include <string>
-
-#include <networktables/NetworkTableEntry.h>
-#include <networktables/NetworkTableInstance.h>
-
-#include "frc/commands/InstantCommand.h"
-#include "frc/shuffleboard/ShuffleboardInstance.h"
-#include "frc/shuffleboard/ShuffleboardTab.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-class ShuffleboardTabTest : public testing::Test {
-  void SetUp() override {
-    m_ntInstance = nt::NetworkTableInstance::Create();
-    m_instance = std::make_unique<detail::ShuffleboardInstance>(m_ntInstance);
-    m_tab = &(m_instance->GetTab("Tab"));
-  }
-
- protected:
-  nt::NetworkTableInstance m_ntInstance;
-  ShuffleboardTab* m_tab;
-  std::unique_ptr<detail::ShuffleboardInstance> m_instance;
-};
-
-TEST_F(ShuffleboardTabTest, AddDouble) {
-  auto entry = m_tab->Add("Double", 1.0).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/Double", entry.GetName());
-  EXPECT_FLOAT_EQ(1.0, entry.GetValue()->GetDouble());
-}
-
-TEST_F(ShuffleboardTabTest, AddInteger) {
-  auto entry = m_tab->Add("Int", 1).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/Int", entry.GetName());
-  EXPECT_FLOAT_EQ(1.0, entry.GetValue()->GetDouble());
-}
-
-TEST_F(ShuffleboardTabTest, AddBoolean) {
-  auto entry = m_tab->Add("Bool", false).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/Bool", entry.GetName());
-  EXPECT_FALSE(entry.GetValue()->GetBoolean());
-}
-
-TEST_F(ShuffleboardTabTest, AddString) {
-  auto entry = m_tab->Add("String", "foobar").GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/String", entry.GetName());
-  EXPECT_EQ("foobar", entry.GetValue()->GetString());
-}
-
-TEST_F(ShuffleboardTabTest, AddNamedSendableWithProperties) {
-  InstantCommand sendable("Command");
-  std::string widgetType = "Command Widget";
-  wpi::StringMap<std::shared_ptr<nt::Value>> map;
-  map.try_emplace("foo", nt::Value::MakeDouble(1234));
-  map.try_emplace("bar", nt::Value::MakeString("baz"));
-  m_tab->Add(sendable).WithWidget(widgetType).WithProperties(map);
-
-  m_instance->Update();
-  std::string meta = "/Shuffleboard/.metadata/Tab/Command";
-
-  EXPECT_EQ(1234, m_ntInstance.GetEntry(meta + "/Properties/foo").GetDouble(-1))
-      << "Property 'foo' not set correctly";
-  EXPECT_EQ("baz",
-            m_ntInstance.GetEntry(meta + "/Properties/bar").GetString(""))
-      << "Property 'bar' not set correctly";
-  EXPECT_EQ(widgetType,
-            m_ntInstance.GetEntry(meta + "/PreferredComponent").GetString(""))
-      << "Preferred component not set correctly";
-}
-
-TEST_F(ShuffleboardTabTest, AddNumberArray) {
-  std::array<double, 3> expect = {{1.0, 2.0, 3.0}};
-  auto entry = m_tab->Add("DoubleArray", expect).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/DoubleArray", entry.GetName());
-
-  auto actual = entry.GetValue()->GetDoubleArray();
-  EXPECT_EQ(expect.size(), actual.size());
-  for (size_t i = 0; i < expect.size(); i++) {
-    EXPECT_FLOAT_EQ(expect[i], actual[i]);
-  }
-}
-
-TEST_F(ShuffleboardTabTest, AddBooleanArray) {
-  std::array<bool, 2> expect = {{true, false}};
-  auto entry = m_tab->Add("BoolArray", expect).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/BoolArray", entry.GetName());
-
-  auto actual = entry.GetValue()->GetBooleanArray();
-  EXPECT_EQ(expect.size(), actual.size());
-  for (size_t i = 0; i < expect.size(); i++) {
-    EXPECT_EQ(expect[i], actual[i]);
-  }
-}
-
-TEST_F(ShuffleboardTabTest, AddStringArray) {
-  std::array<std::string, 2> expect = {{"foo", "bar"}};
-  auto entry = m_tab->Add("StringArray", expect).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/StringArray", entry.GetName());
-
-  auto actual = entry.GetValue()->GetStringArray();
-  EXPECT_EQ(expect.size(), actual.size());
-  for (size_t i = 0; i < expect.size(); i++) {
-    EXPECT_EQ(expect[i], actual[i]);
-  }
-}
diff --git a/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp b/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
deleted file mode 100644
index 0b06d7f..0000000
--- a/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
+++ /dev/null
@@ -1,65 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <array>
-#include <memory>
-#include <string>
-
-#include <networktables/NetworkTableEntry.h>
-#include <networktables/NetworkTableInstance.h>
-
-#include "frc/commands/InstantCommand.h"
-#include "frc/shuffleboard/BuiltInWidgets.h"
-#include "frc/shuffleboard/ShuffleboardInstance.h"
-#include "frc/shuffleboard/ShuffleboardTab.h"
-#include "frc/shuffleboard/ShuffleboardWidget.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-class ShuffleboardWidgetTest : public testing::Test {
-  void SetUp() override {
-    m_ntInstance = nt::NetworkTableInstance::Create();
-    m_instance = std::make_unique<detail::ShuffleboardInstance>(m_ntInstance);
-    m_tab = &(m_instance->GetTab("Tab"));
-  }
-
- protected:
-  nt::NetworkTableInstance m_ntInstance;
-  ShuffleboardTab* m_tab;
-  std::unique_ptr<detail::ShuffleboardInstance> m_instance;
-};
-
-TEST_F(ShuffleboardWidgetTest, UseBuiltInWidget) {
-  auto entry =
-      m_tab->Add("Name", "").WithWidget(BuiltInWidgets::kTextView).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/Name", entry.GetName())
-      << "The widget entry has the wrong name";
-}
-
-TEST_F(ShuffleboardWidgetTest, WithProperties) {
-  wpi::StringMap<std::shared_ptr<nt::Value>> properties{
-      std::make_pair("min", nt::Value::MakeDouble(0)),
-      std::make_pair("max", nt::Value::MakeDouble(1))};
-  auto entry =
-      m_tab->Add("WithProperties", "").WithProperties(properties).GetEntry();
-
-  // Update the instance to generate
-  // the metadata entries for the widget properties
-  m_instance->Update();
-
-  auto propertiesTable = m_ntInstance.GetTable(
-      "/Shuffleboard/.metadata/Tab/WithProperties/Properties");
-
-  EXPECT_EQ("/Shuffleboard/Tab/WithProperties", entry.GetName())
-      << "The widget entry has the wrong name";
-  EXPECT_FLOAT_EQ(0, propertiesTable->GetEntry("min").GetDouble(-1))
-      << "The 'min' property should be 0";
-  EXPECT_FLOAT_EQ(1, propertiesTable->GetEntry("max").GetDouble(-1))
-      << "The 'max' property should be 1";
-}
diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index 8254dd4..319be79 100644
--- a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -73,6 +73,22 @@
     EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
                 a.Rotation().Radians().to<double>(), 1E-9);
 
+    // Check interior waypoints
+    bool interiorsGood = true;
+    for (auto& waypoint : waypoints) {
+      bool found = false;
+      for (auto& state : poses) {
+        if (std::abs(
+                waypoint.Distance(state.first.Translation()).to<double>()) <
+            1E-9) {
+          found = true;
+        }
+      }
+      interiorsGood &= found;
+    }
+
+    EXPECT_TRUE(interiorsGood);
+
     // Check last point.
     EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
                 b.Translation().X().to<double>(), 1E-9);
@@ -97,3 +113,10 @@
   Pose2d end{3_m, 0_m, Rotation2d{90_deg}};
   Run(start, waypoints, end);
 }
+
+TEST_F(CubicHermiteSplineTest, OneInterior) {
+  Pose2d start{0_m, 0_m, 0_rad};
+  std::vector<Translation2d> waypoints{Translation2d(2_m, 0_m)};
+  Pose2d end{4_m, 0_m, 0_rad};
+  Run(start, waypoints, end);
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
new file mode 100644
index 0000000..eb230da
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <iostream>
+#include <memory>
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(DifferentialDriveVoltageConstraintTest, Constraint) {
+  // Pick an unreasonably large kA to ensure the constraint has to do some work
+  SimpleMotorFeedforward<units::meter> feedforward{1_V, 1_V / 1_mps,
+                                                   3_V / 1_mps_sq};
+  const DifferentialDriveKinematics kinematics{0.5_m};
+  const auto maxVoltage = 10_V;
+
+  auto config = TrajectoryConfig(12_fps, 12_fps_sq);
+  config.AddConstraint(
+      DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage));
+
+  auto trajectory = TestTrajectory::GetTrajectory(config);
+
+  units::second_t time = 0_s;
+  units::second_t dt = 20_ms;
+  units::second_t duration = trajectory.TotalTime();
+
+  while (time < duration) {
+    const Trajectory::State point = trajectory.Sample(time);
+    time += dt;
+
+    const ChassisSpeeds chassisSpeeds{point.velocity, 0_mps,
+                                      point.velocity * point.curvature};
+
+    auto [left, right] = kinematics.ToWheelSpeeds(chassisSpeeds);
+
+    // Not really a strictly-correct test as we're using the chassis accel
+    // instead of the wheel accel, but much easier than doing it "properly" and
+    // a reasonable check anyway
+    EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) <
+                maxVoltage + 0.05_V);
+    EXPECT_TRUE(feedforward.Calculate(left, point.acceleration) >
+                -maxVoltage - 0.05_V);
+    EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) <
+                maxVoltage + 0.05_V);
+    EXPECT_TRUE(feedforward.Calculate(right, point.acceleration) >
+                -maxVoltage - 0.05_V);
+  }
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
new file mode 100644
index 0000000..999d2bb
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryUtil.h"
+#include "gtest/gtest.h"
+#include "trajectory/TestTrajectory.h"
+
+using namespace frc;
+
+TEST(TrajectoryJsonTest, DeserializeMatches) {
+  TrajectoryConfig config{12_fps, 12_fps_sq};
+  auto trajectory = TestTrajectory::GetTrajectory(config);
+
+  Trajectory deserialized;
+  EXPECT_NO_THROW(deserialized = TrajectoryUtil::DeserializeTrajectory(
+                      TrajectoryUtil::SerializeTrajectory(trajectory)));
+  EXPECT_EQ(trajectory.States(), deserialized.States());
+}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
index ddd6a70..bd9ffef 100644
--- a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -25,12 +25,13 @@
   }
 
 TEST(TrapezoidProfileTest, ReachesGoal) {
-  frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{3_m, 0_mps};
-  frc::TrapezoidProfile::State state;
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{3_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
 
   for (int i = 0; i < 450; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_EQ(state, goal);
@@ -39,10 +40,11 @@
 // Tests that decreasing the maximum velocity in the middle when it is already
 // moving faster than the new max is handled correctly
 TEST(TrapezoidProfileTest, PosContinousUnderVelChange) {
-  frc::TrapezoidProfile::Constraints constraints{1.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{12_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{1.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
 
-  frc::TrapezoidProfile profile{constraints, goal};
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
   auto state = profile.Calculate(kDt);
 
   auto lastPos = state.position;
@@ -51,7 +53,7 @@
       constraints.maxVelocity = 0.75_mps;
     }
 
-    profile = frc::TrapezoidProfile{constraints, goal, state};
+    profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
     state = profile.Calculate(kDt);
     auto estimatedVel = (state.position - lastPos) / kDt;
 
@@ -71,31 +73,33 @@
 
 // There is some somewhat tricky code for dealing with going backwards
 TEST(TrapezoidProfileTest, Backwards) {
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
-  frc::TrapezoidProfile::State state;
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
 
   for (int i = 0; i < 400; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_EQ(state, goal);
 }
 
 TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
-  frc::TrapezoidProfile::State state;
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
 
   for (int i = 0; i < 200; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_NE(state, goal);
 
   goal = {0.0_m, 0.0_mps};
   for (int i = 0; i < 550; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_EQ(state, goal);
@@ -103,30 +107,32 @@
 
 // Checks to make sure that it hits top speed
 TEST(TrapezoidProfileTest, TopSpeed) {
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{4_m, 0_mps};
-  frc::TrapezoidProfile::State state;
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{4_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
 
   for (int i = 0; i < 200; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_NEAR_UNITS(constraints.maxVelocity, state.velocity, 10e-5_mps);
 
   for (int i = 0; i < 2000; ++i) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
   }
   EXPECT_EQ(state, goal);
 }
 
 TEST(TrapezoidProfileTest, TimingToCurrent) {
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{2_m, 0_mps};
-  frc::TrapezoidProfile::State state;
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::State state;
 
   for (int i = 0; i < 400; i++) {
-    frc::TrapezoidProfile profile{constraints, goal, state};
+    frc::TrapezoidProfile<units::meter> profile{constraints, goal, state};
     state = profile.Calculate(kDt);
     EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
   }
@@ -135,16 +141,17 @@
 TEST(TrapezoidProfileTest, TimingToGoal) {
   using units::unit_cast;
 
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
 
-  frc::TrapezoidProfile profile{constraints, goal};
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
   auto state = profile.Calculate(kDt);
 
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile(constraints, goal, state);
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
     state = profile.Calculate(kDt);
     if (!reachedGoal && state == goal) {
       // Expected value using for loop index is just an approximation since the
@@ -158,16 +165,17 @@
 TEST(TrapezoidProfileTest, TimingBeforeGoal) {
   using units::unit_cast;
 
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{2_m, 0_mps};
 
-  frc::TrapezoidProfile profile{constraints, goal};
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
   auto state = profile.Calculate(kDt);
 
   auto predictedTimeLeft = profile.TimeLeftUntil(1_m);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile(constraints, goal, state);
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
     state = profile.Calculate(kDt);
     if (!reachedGoal &&
         (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
@@ -180,16 +188,17 @@
 TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
   using units::unit_cast;
 
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
 
-  frc::TrapezoidProfile profile{constraints, goal};
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
   auto state = profile.Calculate(kDt);
 
   auto predictedTimeLeft = profile.TimeLeftUntil(goal.position);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile(constraints, goal, state);
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
     state = profile.Calculate(kDt);
     if (!reachedGoal && state == goal) {
       // Expected value using for loop index is just an approximation since the
@@ -203,16 +212,17 @@
 TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
   using units::unit_cast;
 
-  frc::TrapezoidProfile::Constraints constraints{0.75_mps, 0.75_mps_sq};
-  frc::TrapezoidProfile::State goal{-2_m, 0_mps};
+  frc::TrapezoidProfile<units::meter>::Constraints constraints{0.75_mps,
+                                                               0.75_mps_sq};
+  frc::TrapezoidProfile<units::meter>::State goal{-2_m, 0_mps};
 
-  frc::TrapezoidProfile profile{constraints, goal};
+  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
   auto state = profile.Calculate(kDt);
 
   auto predictedTimeLeft = profile.TimeLeftUntil(-1_m);
   bool reachedGoal = false;
   for (int i = 0; i < 400; i++) {
-    profile = frc::TrapezoidProfile(constraints, goal, state);
+    profile = frc::TrapezoidProfile<units::meter>(constraints, goal, state);
     state = profile.Calculate(kDt);
     if (!reachedGoal &&
         (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {