diff --git a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
index a760daa..aff0802 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -21,7 +21,7 @@
     m_simRange =
         m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
     m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
-    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simY = m_simDevice.CreateDouble("Y Accel", false, 0.0);
     m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
   }
   // Turn on the measurements
diff --git a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
index 077a7ff..329b148 100644
--- a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -20,7 +20,7 @@
     m_simRange =
         m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
     m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
-    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simY = m_simDevice.CreateDouble("Y Accel", false, 0.0);
     m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
   }
   m_spi.SetClockRate(500000);
diff --git a/wpilibc/src/main/native/cpp/ADXL362.cpp b/wpilibc/src/main/native/cpp/ADXL362.cpp
index 6ed8c8c..aff572f 100644
--- a/wpilibc/src/main/native/cpp/ADXL362.cpp
+++ b/wpilibc/src/main/native/cpp/ADXL362.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -40,7 +40,7 @@
     m_simRange =
         m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
     m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
-    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simY = m_simDevice.CreateDouble("Y Accel", false, 0.0);
     m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
   }
 
diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
index 2cf2f73..8ea5b41 100644
--- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
+++ b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 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.                                                               */
@@ -32,7 +32,7 @@
 ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
 
 ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port)
-    : m_spi(port), m_simDevice("ADXRS450_Gyro", port) {
+    : m_spi(port), m_port(port), m_simDevice("ADXRS450_Gyro", port) {
   if (m_simDevice) {
     m_simAngle = m_simDevice.CreateDouble("Angle", false, 0.0);
     m_simRate = m_simDevice.CreateDouble("Rate", false, 0.0);
@@ -123,3 +123,5 @@
   m_spi.SetAccumulatorIntegratedCenter(m_spi.GetAccumulatorIntegratedAverage());
   m_spi.ResetAccumulator();
 }
+
+int ADXRS450_Gyro::GetPort() const { return m_port; }
diff --git a/wpilibc/src/main/native/cpp/AddressableLED.cpp b/wpilibc/src/main/native/cpp/AddressableLED.cpp
index b0c3a45..81adc7b 100644
--- a/wpilibc/src/main/native/cpp/AddressableLED.cpp
+++ b/wpilibc/src/main/native/cpp/AddressableLED.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -94,36 +94,5 @@
 }
 
 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;
-  }
+  SetLED(Color::FromHSV(h, s, v));
 }
diff --git a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
index be0c7d2..09bc2f5 100644
--- a/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,6 +9,7 @@
 
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/Base.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
@@ -52,8 +53,8 @@
 
 void AnalogAccelerometer::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Accelerometer");
-  builder.AddDoubleProperty("Value", [=]() { return GetAcceleration(); },
-                            nullptr);
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetAcceleration(); }, nullptr);
 }
 
 void AnalogAccelerometer::InitAccelerometer() {
diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
index a194961..c1f4e8d 100644
--- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -8,6 +8,7 @@
 #include "frc/AnalogEncoder.h"
 
 #include "frc/AnalogInput.h"
+#include "frc/Base.h"
 #include "frc/Counter.h"
 #include "frc/DriverStation.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -94,11 +95,13 @@
   m_positionOffset = m_analogInput->GetVoltage();
 }
 
+int AnalogEncoder::GetChannel() const { return m_analogInput->GetChannel(); }
+
 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);
+  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 c891506..2ec4439 100644
--- a/wpilibc/src/main/native/cpp/AnalogGyro.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogGyro.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -15,6 +15,7 @@
 #include <hal/FRCUsageReporting.h>
 
 #include "frc/AnalogInput.h"
+#include "frc/Base.h"
 #include "frc/Timer.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableRegistry.h"
@@ -160,3 +161,7 @@
   HAL_CalibrateAnalogGyro(m_gyroHandle, &status);
   wpi_setHALError(status);
 }
+
+std::shared_ptr<AnalogInput> AnalogGyro::GetAnalogInput() const {
+  return m_analog;
+}
diff --git a/wpilibc/src/main/native/cpp/AnalogInput.cpp b/wpilibc/src/main/native/cpp/AnalogInput.cpp
index c8197b7..11c30ee 100644
--- a/wpilibc/src/main/native/cpp/AnalogInput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogInput.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,8 +7,6 @@
 
 #include "frc/AnalogInput.h"
 
-#include <utility>
-
 #include <hal/AnalogAccumulator.h>
 #include <hal/AnalogInput.h>
 #include <hal/FRCUsageReporting.h>
@@ -230,6 +228,6 @@
 
 void AnalogInput::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Analog Input");
-  builder.AddDoubleProperty("Value", [=]() { return GetAverageVoltage(); },
-                            nullptr);
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetAverageVoltage(); }, nullptr);
 }
diff --git a/wpilibc/src/main/native/cpp/AnalogOutput.cpp b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
index 041b446..bd8126a 100644
--- a/wpilibc/src/main/native/cpp/AnalogOutput.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogOutput.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 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.                                                               */
@@ -65,10 +65,11 @@
   return voltage;
 }
 
-int AnalogOutput::GetChannel() { return m_channel; }
+int AnalogOutput::GetChannel() const { return m_channel; }
 
 void AnalogOutput::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Analog Output");
-  builder.AddDoubleProperty("Value", [=]() { return GetVoltage(); },
-                            [=](double value) { SetVoltage(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetVoltage(); },
+      [=](double value) { SetVoltage(value); });
 }
diff --git a/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp b/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
index b650ec7..c7aedd6 100644
--- a/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 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,7 @@
 
 #include "frc/AnalogPotentiometer.h"
 
+#include "frc/Base.h"
 #include "frc/RobotController.h"
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
@@ -34,7 +35,8 @@
 }
 
 double AnalogPotentiometer::Get() const {
-  return (m_analog_input->GetVoltage() / RobotController::GetVoltage5V()) *
+  return (m_analog_input->GetAverageVoltage() /
+          RobotController::GetVoltage5V()) *
              m_fullRange +
          m_offset;
 }
@@ -42,5 +44,7 @@
 double AnalogPotentiometer::PIDGet() { return Get(); }
 
 void AnalogPotentiometer::InitSendable(SendableBuilder& builder) {
-  m_analog_input->InitSendable(builder);
+  builder.SetSmartDashboardType("Analog Input");
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, nullptr);
 }
diff --git a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
index ddbb7c8..45821ba 100644
--- a/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
+++ b/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,6 +12,7 @@
 #include <hal/FRCUsageReporting.h>
 
 #include "frc/AnalogInput.h"
+#include "frc/Base.h"
 #include "frc/DutyCycle.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableRegistry.h"
diff --git a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
index d7cdef6..efc0fe6 100644
--- a/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
+++ b/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 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.                                                               */
@@ -43,7 +43,10 @@
 
 void BuiltInAccelerometer::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("3AxisAccelerometer");
-  builder.AddDoubleProperty("X", [=]() { return GetX(); }, nullptr);
-  builder.AddDoubleProperty("Y", [=]() { return GetY(); }, nullptr);
-  builder.AddDoubleProperty("Z", [=]() { return GetZ(); }, nullptr);
+  builder.AddDoubleProperty(
+      "X", [=]() { return GetX(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Y", [=]() { return GetY(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Z", [=]() { return GetZ(); }, nullptr);
 }
diff --git a/wpilibc/src/main/native/cpp/CAN.cpp b/wpilibc/src/main/native/cpp/CAN.cpp
index 797b9ff..e56e435 100644
--- a/wpilibc/src/main/native/cpp/CAN.cpp
+++ b/wpilibc/src/main/native/cpp/CAN.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 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.                                                               */
@@ -70,6 +70,25 @@
   wpi_setHALError(status);
 }
 
+int CAN::WritePacketNoError(const uint8_t* data, int length, int apiId) {
+  int32_t status = 0;
+  HAL_WriteCANPacket(m_handle, data, length, apiId, &status);
+  return status;
+}
+
+int CAN::WritePacketRepeatingNoError(const uint8_t* data, int length, int apiId,
+                                     int repeatMs) {
+  int32_t status = 0;
+  HAL_WriteCANPacketRepeating(m_handle, data, length, apiId, repeatMs, &status);
+  return status;
+}
+
+int CAN::WriteRTRFrameNoError(int length, int apiId) {
+  int32_t status = 0;
+  HAL_WriteCANRTRFrame(m_handle, length, apiId, &status);
+  return status;
+}
+
 void CAN::StopPacketRepeating(int apiId) {
   int32_t status = 0;
   HAL_StopCANPacketRepeating(m_handle, apiId, &status);
diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp
index 29789be..e40758f 100644
--- a/wpilibc/src/main/native/cpp/Compressor.cpp
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 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.                                                               */
@@ -204,15 +204,18 @@
   }
 }
 
+int Compressor::GetModule() const { return m_module; }
+
 void Compressor::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Compressor");
-  builder.AddBooleanProperty("Enabled", [=]() { return Enabled(); },
-                             [=](bool value) {
-                               if (value)
-                                 Start();
-                               else
-                                 Stop();
-                             });
+  builder.AddBooleanProperty(
+      "Enabled", [=]() { return Enabled(); },
+      [=](bool value) {
+        if (value)
+          Start();
+        else
+          Stop();
+      });
   builder.AddBooleanProperty(
       "Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr);
 }
diff --git a/wpilibc/src/main/native/cpp/Counter.cpp b/wpilibc/src/main/native/cpp/Counter.cpp
index d2158ca..ca74ab8 100644
--- a/wpilibc/src/main/native/cpp/Counter.cpp
+++ b/wpilibc/src/main/native/cpp/Counter.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,7 @@
 #include <hal/FRCUsageReporting.h>
 
 #include "frc/AnalogTrigger.h"
+#include "frc/Base.h"
 #include "frc/DigitalInput.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -332,5 +333,6 @@
 
 void Counter::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Counter");
-  builder.AddDoubleProperty("Value", [=]() { return Get(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, nullptr);
 }
diff --git a/wpilibc/src/main/native/cpp/DigitalInput.cpp b/wpilibc/src/main/native/cpp/DigitalInput.cpp
index 7210750..d82f570 100644
--- a/wpilibc/src/main/native/cpp/DigitalInput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -8,7 +8,6 @@
 #include "frc/DigitalInput.h"
 
 #include <limits>
-#include <utility>
 
 #include <hal/DIO.h>
 #include <hal/FRCUsageReporting.h>
@@ -73,5 +72,6 @@
 
 void DigitalInput::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Digital Input");
-  builder.AddBooleanProperty("Value", [=]() { return Get(); }, nullptr);
+  builder.AddBooleanProperty(
+      "Value", [=]() { return Get(); }, nullptr);
 }
diff --git a/wpilibc/src/main/native/cpp/DigitalOutput.cpp b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
index 6e9b09b..0bdc57c 100644
--- a/wpilibc/src/main/native/cpp/DigitalOutput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalOutput.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -8,7 +8,6 @@
 #include "frc/DigitalOutput.h"
 
 #include <limits>
-#include <utility>
 
 #include <hal/DIO.h>
 #include <hal/FRCUsageReporting.h>
@@ -154,6 +153,6 @@
 
 void DigitalOutput::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Digital Output");
-  builder.AddBooleanProperty("Value", [=]() { return Get(); },
-                             [=](bool value) { Set(value); });
+  builder.AddBooleanProperty(
+      "Value", [=]() { return Get(); }, [=](bool value) { Set(value); });
 }
diff --git a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
index c896ea8..67eb0b7 100644
--- a/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
+++ b/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -92,6 +92,7 @@
 
   bool forward = false;
   bool reverse = false;
+
   switch (value) {
     case kOff:
       forward = false;
@@ -106,6 +107,7 @@
       reverse = true;
       break;
   }
+
   int fstatus = 0;
   HAL_SetSolenoid(m_forwardHandle, forward, &fstatus);
   int rstatus = 0;
@@ -117,6 +119,7 @@
 
 DoubleSolenoid::Value DoubleSolenoid::Get() const {
   if (StatusIsFatal()) return kOff;
+
   int fstatus = 0;
   int rstatus = 0;
   bool valueForward = HAL_GetSolenoid(m_forwardHandle, &fstatus);
@@ -125,9 +128,23 @@
   wpi_setHALError(fstatus);
   wpi_setHALError(rstatus);
 
-  if (valueForward) return kForward;
-  if (valueReverse) return kReverse;
-  return kOff;
+  if (valueForward) {
+    return kForward;
+  } else if (valueReverse) {
+    return kReverse;
+  } else {
+    return kOff;
+  }
+}
+
+void DoubleSolenoid::Toggle() {
+  Value value = Get();
+
+  if (value == kForward) {
+    Set(kReverse);
+  } else if (value == kReverse) {
+    Set(kForward);
+  }
 }
 
 bool DoubleSolenoid::IsFwdSolenoidBlackListed() const {
diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp
index 7c64883..dbb99f9 100644
--- a/wpilibc/src/main/native/cpp/DriverStation.cpp
+++ b/wpilibc/src/main/native/cpp/DriverStation.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,6 @@
 #include <chrono>
 
 #include <hal/DriverStation.h>
-#include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <hal/Power.h>
 #include <networktables/NetworkTable.h>
@@ -19,10 +18,8 @@
 #include <wpi/SmallString.h>
 #include <wpi/StringRef.h>
 
-#include "frc/AnalogInput.h"
 #include "frc/MotorSafety.h"
 #include "frc/Timer.h"
-#include "frc/Utility.h"
 #include "frc/WPIErrors.h"
 
 namespace frc {
@@ -68,6 +65,15 @@
 
 static constexpr double kJoystickUnpluggedMessageInterval = 1.0;
 
+static int& GetDSLastCount() {
+  // There is a rollover error condition here. At Packet# = n * (uintmax), this
+  // will return false when instead it should return true. However, this at a
+  // 20ms rate occurs once every 2.7 years of DS connected runtime, so not
+  // worth the cycles to check.
+  thread_local int lastCount{0};
+  return lastCount;
+}
+
 DriverStation::~DriverStation() {
   m_isRunning = false;
   // Trigger a DS mutex release in case there is no driver station running.
@@ -325,6 +331,11 @@
   return static_cast<bool>(descriptor.axisTypes);
 }
 
+bool DriverStation::IsJoystickConnected(int stick) const {
+  return GetStickAxisCount(stick) > 0 || GetStickButtonCount(stick) > 0 ||
+         GetStickPOVCount(stick) > 0;
+}
+
 bool DriverStation::IsEnabled() const {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
@@ -349,12 +360,24 @@
   return controlWord.autonomous;
 }
 
+bool DriverStation::IsAutonomousEnabled() const {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return controlWord.autonomous && controlWord.enabled;
+}
+
 bool DriverStation::IsOperatorControl() const {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return !(controlWord.autonomous || controlWord.test);
 }
 
+bool DriverStation::IsOperatorControlEnabled() const {
+  HAL_ControlWord controlWord;
+  HAL_GetControlWord(&controlWord);
+  return !controlWord.autonomous && !controlWord.test && controlWord.enabled;
+}
+
 bool DriverStation::IsTest() const {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
@@ -367,7 +390,14 @@
   return controlWord.dsAttached;
 }
 
-bool DriverStation::IsNewControlData() const { return HAL_IsNewControlData(); }
+bool DriverStation::IsNewControlData() const {
+  std::unique_lock lock(m_waitForDataMutex);
+  int& lastCount = GetDSLastCount();
+  int currentCount = m_waitForDataCounter;
+  if (lastCount == currentCount) return false;
+  lastCount = currentCount;
+  return true;
+}
 
 bool DriverStation::IsFMSAttached() const {
   HAL_ControlWord controlWord;
@@ -448,7 +478,12 @@
       std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
 
   std::unique_lock lock(m_waitForDataMutex);
+  int& lastCount = GetDSLastCount();
   int currentCount = m_waitForDataCounter;
+  if (lastCount != currentCount) {
+    lastCount = currentCount;
+    return true;
+  }
   while (m_waitForDataCounter == currentCount) {
     if (timeout > 0) {
       auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime);
@@ -459,6 +494,7 @@
       m_waitForDataCond.wait(lock);
     }
   }
+  lastCount = m_waitForDataCounter;
   return true;
 }
 
@@ -507,6 +543,14 @@
   SendMatchData();
 }
 
+void DriverStation::SilenceJoystickConnectionWarning(bool silence) {
+  m_silenceJoystickWarning = silence;
+}
+
+bool DriverStation::IsJoystickConnectionWarningSilenced() const {
+  return !IsFMSAttached() && m_silenceJoystickWarning;
+}
+
 DriverStation::DriverStation() {
   HAL_Initialize(500, 0);
   m_waitForDataCounter = 0;
@@ -534,10 +578,12 @@
 }
 
 void DriverStation::ReportJoystickUnpluggedWarning(const wpi::Twine& message) {
-  double currentTime = Timer::GetFPGATimestamp();
-  if (currentTime > m_nextMessageTime) {
-    ReportWarning(message);
-    m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
+  if (IsFMSAttached() || !m_silenceJoystickWarning) {
+    double currentTime = Timer::GetFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      ReportWarning(message);
+      m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
+    }
   }
 }
 
diff --git a/wpilibc/src/main/native/cpp/DutyCycle.cpp b/wpilibc/src/main/native/cpp/DutyCycle.cpp
index 12f390e..7e4e98d 100644
--- a/wpilibc/src/main/native/cpp/DutyCycle.cpp
+++ b/wpilibc/src/main/native/cpp/DutyCycle.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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,6 +10,7 @@
 #include <hal/DutyCycle.h>
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/Base.h"
 #include "frc/DigitalSource.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -93,8 +94,8 @@
 
 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);
+  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
index ea054ce..4fc5457 100644
--- a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
+++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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,7 @@
 
 #include "frc/DutyCycleEncoder.h"
 
+#include "frc/Base.h"
 #include "frc/Counter.h"
 #include "frc/DigitalInput.h"
 #include "frc/DigitalSource.h"
@@ -18,51 +19,37 @@
 
 DutyCycleEncoder::DutyCycleEncoder(int channel)
     : m_dutyCycle{std::make_shared<DutyCycle>(
-          std::make_shared<DigitalInput>(channel))},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+          std::make_shared<DigitalInput>(channel))} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
-    : m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle)
-    : m_dutyCycle{dutyCycle, NullDeleter<DutyCycle>{}},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{dutyCycle, NullDeleter<DutyCycle>{}} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle)
-    : m_dutyCycle{std::move(dutyCycle)},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{std::move(dutyCycle)} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource)
-    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource)
-    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource)
-    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)},
-      m_analogTrigger{m_dutyCycle.get()},
-      m_counter{} {
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
   Init();
 }
 
@@ -71,15 +58,19 @@
 
   if (m_simDevice) {
     m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
+    m_simDistancePerRotation =
+        m_simDevice.CreateDouble("DistancePerRotation", false, 1.0);
     m_simIsConnected = m_simDevice.CreateBoolean("Connected", false, true);
+  } else {
+    m_analogTrigger = std::make_unique<AnalogTrigger>(m_dutyCycle.get());
+    m_analogTrigger->SetLimitsDutyCycle(0.25, 0.75);
+    m_counter = std::make_unique<Counter>();
+    m_counter->SetUpSource(
+        m_analogTrigger->CreateOutput(AnalogTriggerType::kRisingPulse));
+    m_counter->SetDownSource(
+        m_analogTrigger->CreateOutput(AnalogTriggerType::kFallingPulse));
   }
 
-  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());
 }
@@ -90,9 +81,9 @@
   // 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 counter = m_counter->Get();
     auto pos = m_dutyCycle->GetOutput();
-    auto counter2 = m_counter.Get();
+    auto counter2 = m_counter->Get();
     auto pos2 = m_dutyCycle->GetOutput();
     if (counter == counter2 && pos == pos2) {
       units::turn_t turns{counter + pos - m_positionOffset};
@@ -109,6 +100,7 @@
 
 void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) {
   m_distancePerRotation = distancePerRotation;
+  m_simDistancePerRotation.Set(distancePerRotation);
 }
 
 double DutyCycleEncoder::GetDistancePerRotation() const {
@@ -124,7 +116,7 @@
 }
 
 void DutyCycleEncoder::Reset() {
-  m_counter.Reset();
+  if (m_counter) m_counter->Reset();
   m_positionOffset = m_dutyCycle->GetOutput();
 }
 
@@ -140,13 +132,21 @@
   m_frequencyThreshold = frequency;
 }
 
+int DutyCycleEncoder::GetFPGAIndex() const {
+  return m_dutyCycle->GetFPGAIndex();
+}
+
+int DutyCycleEncoder::GetSourceChannel() const {
+  return m_dutyCycle->GetSourceChannel();
+}
+
 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);
+  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 963bcc6..87bb45f 100644
--- a/wpilibc/src/main/native/cpp/Encoder.cpp
+++ b/wpilibc/src/main/native/cpp/Encoder.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,6 +12,7 @@
 #include <hal/Encoder.h>
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/Base.h"
 #include "frc/DigitalInput.h"
 #include "frc/WPIErrors.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -236,11 +237,12 @@
   else
     builder.SetSmartDashboardType("Encoder");
 
-  builder.AddDoubleProperty("Speed", [=]() { return GetRate(); }, nullptr);
-  builder.AddDoubleProperty("Distance", [=]() { return GetDistance(); },
-                            nullptr);
-  builder.AddDoubleProperty("Distance per Tick",
-                            [=]() { return GetDistancePerPulse(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Speed", [=]() { return GetRate(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Distance", [=]() { return GetDistance(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Distance per Tick", [=]() { return GetDistancePerPulse(); }, nullptr);
 }
 
 void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) {
diff --git a/wpilibc/src/main/native/cpp/Error.cpp b/wpilibc/src/main/native/cpp/Error.cpp
index ee7dcbd..5e072c9 100644
--- a/wpilibc/src/main/native/cpp/Error.cpp
+++ b/wpilibc/src/main/native/cpp/Error.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,9 +10,9 @@
 #include <wpi/Path.h>
 #include <wpi/StackTrace.h>
 
+#include "frc/Base.h"
 #include "frc/DriverStation.h"
 #include "frc/Timer.h"
-#include "frc/Utility.h"
 
 using namespace frc;
 
diff --git a/wpilibc/src/main/native/cpp/ErrorBase.cpp b/wpilibc/src/main/native/cpp/ErrorBase.cpp
index d098c07..8c7c5a2 100644
--- a/wpilibc/src/main/native/cpp/ErrorBase.cpp
+++ b/wpilibc/src/main/native/cpp/ErrorBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -8,17 +8,17 @@
 #include "frc/ErrorBase.h"
 
 #include <cerrno>
-#include <cstdio>
 #include <cstring>
 #include <set>
+#include <utility>
 
-#include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <wpi/Format.h>
 #include <wpi/SmallString.h>
+#include <wpi/mutex.h>
 #include <wpi/raw_ostream.h>
 
-#include "frc/WPIErrors.h"
+#include "frc/Base.h"
 
 using namespace frc;
 
diff --git a/wpilibc/src/main/native/cpp/Filesystem.cpp b/wpilibc/src/main/native/cpp/Filesystem.cpp
index 1546050..7d2ea1d 100644
--- a/wpilibc/src/main/native/cpp/Filesystem.cpp
+++ b/wpilibc/src/main/native/cpp/Filesystem.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 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.                                                               */
@@ -18,7 +18,7 @@
 
 void frc::filesystem::GetOperatingDirectory(
     wpi::SmallVectorImpl<char>& result) {
-  if (RobotBase::IsReal()) {
+  if constexpr (RobotBase::IsReal()) {
     wpi::sys::path::native("/home/lvuser", result);
   } else {
     frc::filesystem::GetLaunchDirectory(result);
@@ -27,5 +27,11 @@
 
 void frc::filesystem::GetDeployDirectory(wpi::SmallVectorImpl<char>& result) {
   frc::filesystem::GetOperatingDirectory(result);
-  wpi::sys::path::append(result, "deploy");
+  if constexpr (RobotBase::IsReal()) {
+    wpi::sys::path::append(result, "deploy");
+  } else {
+    wpi::sys::path::append(result, "src");
+    wpi::sys::path::append(result, "main");
+    wpi::sys::path::append(result, "deploy");
+  }
 }
diff --git a/wpilibc/src/main/native/cpp/GenericHID.cpp b/wpilibc/src/main/native/cpp/GenericHID.cpp
index a7dcfc0..4a264c8 100644
--- a/wpilibc/src/main/native/cpp/GenericHID.cpp
+++ b/wpilibc/src/main/native/cpp/GenericHID.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 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.                                                               */
@@ -8,7 +8,6 @@
 #include "frc/GenericHID.h"
 
 #include <hal/DriverStation.h>
-#include <hal/FRCUsageReporting.h>
 
 #include "frc/DriverStation.h"
 #include "frc/WPIErrors.h"
@@ -48,6 +47,10 @@
   return m_ds->GetStickButtonCount(m_port);
 }
 
+bool GenericHID::IsConnected() const {
+  return m_ds->IsJoystickConnected(m_port);
+}
+
 GenericHID::HIDType GenericHID::GetType() const {
   return static_cast<HIDType>(m_ds->GetJoystickType(m_port));
 }
diff --git a/wpilibc/src/main/native/cpp/GyroBase.cpp b/wpilibc/src/main/native/cpp/GyroBase.cpp
index ec67675..ba9b2f0 100644
--- a/wpilibc/src/main/native/cpp/GyroBase.cpp
+++ b/wpilibc/src/main/native/cpp/GyroBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -25,5 +25,6 @@
 
 void GyroBase::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Gyro");
-  builder.AddDoubleProperty("Value", [=]() { return GetAngle(); }, nullptr);
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetAngle(); }, nullptr);
 }
diff --git a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
index 202c61d..744e349 100644
--- a/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
+++ b/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,8 +7,6 @@
 
 #include "frc/InterruptableSensorBase.h"
 
-#include <hal/FRCUsageReporting.h>
-
 #include "frc/Utility.h"
 #include "frc/WPIErrors.h"
 
diff --git a/wpilibc/src/main/native/cpp/IterativeRobot.cpp b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
index c8664a5..72fb79b 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobot.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -24,6 +24,10 @@
 void IterativeRobot::StartCompetition() {
   RobotInit();
 
+  if constexpr (IsSimulation()) {
+    SimulationInit();
+  }
+
   // Tell the DS that the robot is ready to be enabled
   HAL_ObserveUserProgramStarting();
 
diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
index 2997234..7b29130 100644
--- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
+++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 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,16 +7,12 @@
 
 #include "frc/IterativeRobotBase.h"
 
-#include <cstdio>
-
 #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/livewindow/LiveWindow.h"
 #include "frc/shuffleboard/Shuffleboard.h"
 #include "frc/smartdashboard/SmartDashboard.h"
@@ -34,6 +30,10 @@
   wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
 }
 
+void IterativeRobotBase::SimulationInit() {
+  wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+}
+
 void IterativeRobotBase::DisabledInit() {
   wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
 }
@@ -58,6 +58,14 @@
   }
 }
 
+void IterativeRobotBase::SimulationPeriodic() {
+  static bool firstRun = true;
+  if (firstRun) {
+    wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+    firstRun = false;
+  }
+}
+
 void IterativeRobotBase::DisabledPeriodic() {
   static bool firstRun = true;
   if (firstRun) {
@@ -161,6 +169,12 @@
   m_watchdog.AddEpoch("LiveWindow::UpdateValues()");
   Shuffleboard::Update();
   m_watchdog.AddEpoch("Shuffleboard::Update()");
+
+  if constexpr (IsSimulation()) {
+    SimulationPeriodic();
+    m_watchdog.AddEpoch("SimulationPeriodic()");
+  }
+
   m_watchdog.Disable();
 
   // Warn on loop time overruns
diff --git a/wpilibc/src/main/native/cpp/Joystick.cpp b/wpilibc/src/main/native/cpp/Joystick.cpp
index 8d464cf..c2cdaea 100644
--- a/wpilibc/src/main/native/cpp/Joystick.cpp
+++ b/wpilibc/src/main/native/cpp/Joystick.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,9 +12,6 @@
 #include <hal/FRCUsageReporting.h>
 #include <wpi/math>
 
-#include "frc/DriverStation.h"
-#include "frc/WPIErrors.h"
-
 using namespace frc;
 
 Joystick::Joystick(int port) : GenericHID(port) {
diff --git a/wpilibc/src/main/native/cpp/NidecBrushless.cpp b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
index 5bce36e..82a278e 100644
--- a/wpilibc/src/main/native/cpp/NidecBrushless.cpp
+++ b/wpilibc/src/main/native/cpp/NidecBrushless.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 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.                                                               */
@@ -70,6 +70,6 @@
   builder.SetSmartDashboardType("Nidec Brushless");
   builder.SetActuator(true);
   builder.SetSafeState([=]() { StopMotor(); });
-  builder.AddDoubleProperty("Value", [=]() { return Get(); },
-                            [=](double value) { Set(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, [=](double value) { Set(value); });
 }
diff --git a/wpilibc/src/main/native/cpp/Notifier.cpp b/wpilibc/src/main/native/cpp/Notifier.cpp
index a7fa038..4280ce4 100644
--- a/wpilibc/src/main/native/cpp/Notifier.cpp
+++ b/wpilibc/src/main/native/cpp/Notifier.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -11,6 +11,7 @@
 
 #include <hal/FRCUsageReporting.h>
 #include <hal/Notifier.h>
+#include <hal/Threads.h>
 #include <wpi/SmallString.h>
 
 #include "frc/Timer.h"
@@ -54,6 +55,42 @@
   });
 }
 
+Notifier::Notifier(int priority, std::function<void()> handler) {
+  if (handler == nullptr)
+    wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
+  m_handler = handler;
+  int32_t status = 0;
+  m_notifier = HAL_InitializeNotifier(&status);
+  wpi_setHALError(status);
+
+  m_thread = std::thread([=] {
+    int32_t status = 0;
+    HAL_SetCurrentThreadPriority(true, priority, &status);
+    for (;;) {
+      HAL_NotifierHandle notifier = m_notifier.load();
+      if (notifier == 0) break;
+      uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
+      if (curTime == 0 || status != 0) break;
+
+      std::function<void()> handler;
+      {
+        std::scoped_lock lock(m_processMutex);
+        handler = m_handler;
+        if (m_periodic) {
+          m_expirationTime += m_period;
+          UpdateAlarm();
+        } else {
+          // need to update the alarm to cause it to wait again
+          UpdateAlarm(UINT64_MAX);
+        }
+      }
+
+      // call callback
+      if (handler) handler();
+    }
+  });
+}
+
 Notifier::~Notifier() {
   int32_t status = 0;
   // atomically set handle to 0, then clean
@@ -129,6 +166,8 @@
 }
 
 void Notifier::Stop() {
+  std::scoped_lock lock(m_processMutex);
+  m_periodic = false;
   int32_t status = 0;
   HAL_CancelNotifierAlarm(m_notifier, &status);
   wpi_setHALError(status);
diff --git a/wpilibc/src/main/native/cpp/PIDBase.cpp b/wpilibc/src/main/native/cpp/PIDBase.cpp
index c8f6fed..5802d98 100644
--- a/wpilibc/src/main/native/cpp/PIDBase.cpp
+++ b/wpilibc/src/main/native/cpp/PIDBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -229,16 +229,17 @@
 void PIDBase::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("PIDController");
   builder.SetSafeState([=]() { Reset(); });
-  builder.AddDoubleProperty("p", [=]() { return GetP(); },
-                            [=](double value) { SetP(value); });
-  builder.AddDoubleProperty("i", [=]() { return GetI(); },
-                            [=](double value) { SetI(value); });
-  builder.AddDoubleProperty("d", [=]() { return GetD(); },
-                            [=](double value) { SetD(value); });
-  builder.AddDoubleProperty("f", [=]() { return GetF(); },
-                            [=](double value) { SetF(value); });
-  builder.AddDoubleProperty("setpoint", [=]() { return GetSetpoint(); },
-                            [=](double value) { SetSetpoint(value); });
+  builder.AddDoubleProperty(
+      "p", [=]() { return GetP(); }, [=](double value) { SetP(value); });
+  builder.AddDoubleProperty(
+      "i", [=]() { return GetI(); }, [=](double value) { SetI(value); });
+  builder.AddDoubleProperty(
+      "d", [=]() { return GetD(); }, [=](double value) { SetD(value); });
+  builder.AddDoubleProperty(
+      "f", [=]() { return GetF(); }, [=](double value) { SetF(value); });
+  builder.AddDoubleProperty(
+      "setpoint", [=]() { return GetSetpoint(); },
+      [=](double value) { SetSetpoint(value); });
 }
 
 void PIDBase::Calculate() {
diff --git a/wpilibc/src/main/native/cpp/PIDController.cpp b/wpilibc/src/main/native/cpp/PIDController.cpp
index 0c86f55..a90a645 100644
--- a/wpilibc/src/main/native/cpp/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/PIDController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -80,6 +80,7 @@
 
 void PIDController::InitSendable(SendableBuilder& builder) {
   PIDBase::InitSendable(builder);
-  builder.AddBooleanProperty("enabled", [=]() { return IsEnabled(); },
-                             [=](bool value) { SetEnabled(value); });
+  builder.AddBooleanProperty(
+      "enabled", [=]() { return IsEnabled(); },
+      [=](bool value) { SetEnabled(value); });
 }
diff --git a/wpilibc/src/main/native/cpp/PWM.cpp b/wpilibc/src/main/native/cpp/PWM.cpp
index dabcd2e..c2cd2e2 100644
--- a/wpilibc/src/main/native/cpp/PWM.cpp
+++ b/wpilibc/src/main/native/cpp/PWM.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -199,6 +199,7 @@
   builder.SetSmartDashboardType("PWM");
   builder.SetActuator(true);
   builder.SetSafeState([=]() { SetDisabled(); });
-  builder.AddDoubleProperty("Value", [=]() { return GetRaw(); },
-                            [=](double value) { SetRaw(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetRaw(); },
+      [=](double value) { SetRaw(value); });
 }
diff --git a/wpilibc/src/main/native/cpp/PWMSpeedController.cpp b/wpilibc/src/main/native/cpp/PWMSpeedController.cpp
index ea298de..ec0be7b 100644
--- a/wpilibc/src/main/native/cpp/PWMSpeedController.cpp
+++ b/wpilibc/src/main/native/cpp/PWMSpeedController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -35,6 +35,7 @@
   builder.SetSmartDashboardType("Speed Controller");
   builder.SetActuator(true);
   builder.SetSafeState([=]() { SetDisabled(); });
-  builder.AddDoubleProperty("Value", [=]() { return GetSpeed(); },
-                            [=](double value) { SetSpeed(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetSpeed(); },
+      [=](double value) { SetSpeed(value); });
 }
diff --git a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
index 7d7d916..4070633 100644
--- a/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
+++ b/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 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.                                                               */
@@ -25,7 +25,7 @@
 /**
  * Initialize the PDP.
  */
-PowerDistributionPanel::PowerDistributionPanel(int module) {
+PowerDistributionPanel::PowerDistributionPanel(int module) : m_module(module) {
   int32_t status = 0;
   m_handle = HAL_InitializePDP(module, &status);
   if (status != 0) {
@@ -136,13 +136,16 @@
   }
 }
 
+int PowerDistributionPanel::GetModule() const { return m_module; }
+
 void PowerDistributionPanel::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("PowerDistributionPanel");
   for (int i = 0; i < SensorUtil::kPDPChannels; ++i) {
-    builder.AddDoubleProperty("Chan" + wpi::Twine(i),
-                              [=]() { return GetCurrent(i); }, nullptr);
+    builder.AddDoubleProperty(
+        "Chan" + wpi::Twine(i), [=]() { return GetCurrent(i); }, nullptr);
   }
-  builder.AddDoubleProperty("Voltage", [=]() { return GetVoltage(); }, nullptr);
-  builder.AddDoubleProperty("TotalCurrent", [=]() { return GetTotalCurrent(); },
-                            nullptr);
+  builder.AddDoubleProperty(
+      "Voltage", [=]() { return GetVoltage(); }, nullptr);
+  builder.AddDoubleProperty(
+      "TotalCurrent", [=]() { return GetTotalCurrent(); }, nullptr);
 }
diff --git a/wpilibc/src/main/native/cpp/Preferences.cpp b/wpilibc/src/main/native/cpp/Preferences.cpp
index 6014775..8c1eb87 100644
--- a/wpilibc/src/main/native/cpp/Preferences.cpp
+++ b/wpilibc/src/main/native/cpp/Preferences.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 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.                                                               */
@@ -58,36 +58,66 @@
   entry.SetPersistent();
 }
 
+void Preferences::InitString(wpi::StringRef key, wpi::StringRef value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultString(value);
+}
+
 void Preferences::PutInt(wpi::StringRef key, int value) {
   auto entry = m_table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
+void Preferences::InitInt(wpi::StringRef key, int value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultDouble(value);
+}
+
 void Preferences::PutDouble(wpi::StringRef key, double value) {
   auto entry = m_table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
+void Preferences::InitDouble(wpi::StringRef key, double value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultDouble(value);
+}
+
 void Preferences::PutFloat(wpi::StringRef key, float value) {
   auto entry = m_table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
+void Preferences::InitFloat(wpi::StringRef key, float value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultDouble(value);
+}
+
 void Preferences::PutBoolean(wpi::StringRef key, bool value) {
   auto entry = m_table->GetEntry(key);
   entry.SetBoolean(value);
   entry.SetPersistent();
 }
 
+void Preferences::InitBoolean(wpi::StringRef key, bool value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultBoolean(value);
+}
+
 void Preferences::PutLong(wpi::StringRef key, int64_t value) {
   auto entry = m_table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
+void Preferences::InitLong(wpi::StringRef key, int64_t value) {
+  auto entry = m_table->GetEntry(key);
+  entry.SetDefaultDouble(value);
+}
+
 bool Preferences::ContainsKey(wpi::StringRef key) {
   return m_table->ContainsKey(key);
 }
diff --git a/wpilibc/src/main/native/cpp/RobotController.cpp b/wpilibc/src/main/native/cpp/RobotController.cpp
index b1d7608..b33479f 100644
--- a/wpilibc/src/main/native/cpp/RobotController.cpp
+++ b/wpilibc/src/main/native/cpp/RobotController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 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.                                                               */
@@ -8,7 +8,6 @@
 #include "frc/RobotController.h"
 
 #include <hal/CAN.h>
-#include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <hal/Power.h>
 
diff --git a/wpilibc/src/main/native/cpp/RobotDrive.cpp b/wpilibc/src/main/native/cpp/RobotDrive.cpp
index 5235fa2..9eafe0c 100644
--- a/wpilibc/src/main/native/cpp/RobotDrive.cpp
+++ b/wpilibc/src/main/native/cpp/RobotDrive.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -282,7 +282,7 @@
   double yIn = y;
   // Negate y for the joystick.
   yIn = -yIn;
-  // Compenstate for gyro angle.
+  // Compensate for gyro angle.
   RotateVector(xIn, yIn, gyroAngle);
 
   double wheelSpeeds[kMaxNumberOfMotors];
diff --git a/wpilibc/src/main/native/cpp/ScopedTracer.cpp b/wpilibc/src/main/native/cpp/ScopedTracer.cpp
new file mode 100644
index 0000000..2024a65
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ScopedTracer.cpp
@@ -0,0 +1,22 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/ScopedTracer.h"
+
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+ScopedTracer::ScopedTracer(wpi::Twine name, wpi::raw_ostream& os)
+    : m_name(name.str()), m_os(os) {
+  m_tracer.ResetTimer();
+}
+
+ScopedTracer::~ScopedTracer() {
+  m_tracer.AddEpoch(m_name);
+  m_tracer.PrintEpochs(m_os);
+}
diff --git a/wpilibc/src/main/native/cpp/SensorUtil.cpp b/wpilibc/src/main/native/cpp/SensorUtil.cpp
index 8c02f1b..f86c72c 100644
--- a/wpilibc/src/main/native/cpp/SensorUtil.cpp
+++ b/wpilibc/src/main/native/cpp/SensorUtil.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,6 @@
 #include <hal/AnalogInput.h>
 #include <hal/AnalogOutput.h>
 #include <hal/DIO.h>
-#include <hal/FRCUsageReporting.h>
 #include <hal/PDP.h>
 #include <hal/PWM.h>
 #include <hal/Ports.h>
@@ -21,6 +20,7 @@
 
 const int SensorUtil::kDigitalChannels = HAL_GetNumDigitalChannels();
 const int SensorUtil::kAnalogInputs = HAL_GetNumAnalogInputs();
+const int SensorUtil::kAnalogOutputs = HAL_GetNumAnalogOutputs();
 const int SensorUtil::kSolenoidChannels = HAL_GetNumSolenoidChannels();
 const int SensorUtil::kSolenoidModules = HAL_GetNumPCMModules();
 const int SensorUtil::kPwmChannels = HAL_GetNumPWMChannels();
diff --git a/wpilibc/src/main/native/cpp/Servo.cpp b/wpilibc/src/main/native/cpp/Servo.cpp
index 4b6856a..5edcebc 100644
--- a/wpilibc/src/main/native/cpp/Servo.cpp
+++ b/wpilibc/src/main/native/cpp/Servo.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -57,8 +57,8 @@
 
 void Servo::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Servo");
-  builder.AddDoubleProperty("Value", [=]() { return Get(); },
-                            [=](double value) { Set(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, [=](double value) { Set(value); });
 }
 
 double Servo::GetServoAngleRange() const {
diff --git a/wpilibc/src/main/native/cpp/Solenoid.cpp b/wpilibc/src/main/native/cpp/Solenoid.cpp
index e0bde3c..b5abf20 100644
--- a/wpilibc/src/main/native/cpp/Solenoid.cpp
+++ b/wpilibc/src/main/native/cpp/Solenoid.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -56,6 +56,7 @@
 
 void Solenoid::Set(bool on) {
   if (StatusIsFatal()) return;
+
   int32_t status = 0;
   HAL_SetSolenoid(m_solenoidHandle, on, &status);
   wpi_setHALError(status);
@@ -63,12 +64,16 @@
 
 bool Solenoid::Get() const {
   if (StatusIsFatal()) return false;
+
   int32_t status = 0;
   bool value = HAL_GetSolenoid(m_solenoidHandle, &status);
   wpi_setHALError(status);
+
   return value;
 }
 
+void Solenoid::Toggle() { Set(!Get()); }
+
 bool Solenoid::IsBlackListed() const {
   int value = GetPCMSolenoidBlackList(m_moduleNumber) & (1 << m_channel);
   return (value != 0);
@@ -93,6 +98,6 @@
   builder.SetSmartDashboardType("Solenoid");
   builder.SetActuator(true);
   builder.SetSafeState([=]() { Set(false); });
-  builder.AddBooleanProperty("Value", [=]() { return Get(); },
-                             [=](bool value) { Set(value); });
+  builder.AddBooleanProperty(
+      "Value", [=]() { return Get(); }, [=](bool value) { Set(value); });
 }
diff --git a/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp b/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
index a807afc..8fea1b2 100644
--- a/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
+++ b/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 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.                                                               */
@@ -8,9 +8,27 @@
 #include "frc/SpeedControllerGroup.h"
 
 #include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
+// Can't use a delegated constructor here because of an MSVC bug.
+// https://developercommunity.visualstudio.com/content/problem/583/compiler-bug-with-delegating-a-constructor.html
+
+SpeedControllerGroup::SpeedControllerGroup(
+    std::vector<std::reference_wrapper<SpeedController>>&& speedControllers)
+    : m_speedControllers(std::move(speedControllers)) {
+  Initialize();
+}
+
+void SpeedControllerGroup::Initialize() {
+  for (auto& speedController : m_speedControllers)
+    SendableRegistry::GetInstance().AddChild(this, &speedController.get());
+  static int instances = 0;
+  ++instances;
+  SendableRegistry::GetInstance().Add(this, "SpeedControllerGroup", instances);
+}
+
 void SpeedControllerGroup::Set(double speed) {
   for (auto speedController : m_speedControllers) {
     speedController.get().Set(m_isInverted ? -speed : speed);
@@ -48,6 +66,6 @@
   builder.SetSmartDashboardType("Speed Controller");
   builder.SetActuator(true);
   builder.SetSafeState([=]() { StopMotor(); });
-  builder.AddDoubleProperty("Value", [=]() { return Get(); },
-                            [=](double value) { Set(value); });
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, [=](double value) { Set(value); });
 }
diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp
index a9358dd..74e658a 100644
--- a/wpilibc/src/main/native/cpp/TimedRobot.cpp
+++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 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.                                                               */
@@ -24,24 +24,44 @@
 void TimedRobot::StartCompetition() {
   RobotInit();
 
+  if constexpr (IsSimulation()) {
+    SimulationInit();
+  }
+
   // Tell the DS that the robot is ready to be enabled
   HAL_ObserveUserProgramStarting();
 
-  m_expirationTime = units::second_t{Timer::GetFPGATimestamp()} + m_period;
-  UpdateAlarm();
-
   // Loop forever, calling the appropriate mode-dependent function
   while (true) {
+    // We don't have to check there's an element in the queue first because
+    // there's always at least one (the constructor adds one). It's reenqueued
+    // at the end of the loop.
+    auto callback = m_callbacks.pop();
+
     int32_t status = 0;
+    HAL_UpdateNotifierAlarm(
+        m_notifier, static_cast<uint64_t>(callback.expirationTime * 1e6),
+        &status);
+    wpi_setHALError(status);
+
     uint64_t curTime = HAL_WaitForNotifierAlarm(m_notifier, &status);
     if (curTime == 0 || status != 0) break;
 
-    m_expirationTime += m_period;
+    callback.func();
 
-    UpdateAlarm();
+    callback.expirationTime += callback.period;
+    m_callbacks.push(std::move(callback));
 
-    // Call callback
-    LoopFunc();
+    // Process all other callbacks that are ready to run
+    while (static_cast<uint64_t>(m_callbacks.top().expirationTime * 1e6) <=
+           curTime) {
+      callback = m_callbacks.pop();
+
+      callback.func();
+
+      callback.expirationTime += callback.period;
+      m_callbacks.push(std::move(callback));
+    }
   }
 }
 
@@ -57,6 +77,9 @@
 TimedRobot::TimedRobot(double period) : TimedRobot(units::second_t(period)) {}
 
 TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
+  m_startTime = frc2::Timer::GetFPGATimestamp();
+  AddPeriodic([=] { LoopFunc(); }, period);
+
   int32_t status = 0;
   m_notifier = HAL_InitializeNotifier(&status);
   wpi_setHALError(status);
@@ -75,9 +98,7 @@
   HAL_CleanNotifier(m_notifier, &status);
 }
 
-void TimedRobot::UpdateAlarm() {
-  int32_t status = 0;
-  HAL_UpdateNotifierAlarm(
-      m_notifier, static_cast<uint64_t>(m_expirationTime * 1e6), &status);
-  wpi_setHALError(status);
+void TimedRobot::AddPeriodic(std::function<void()> callback,
+                             units::second_t period, units::second_t offset) {
+  m_callbacks.emplace(callback, m_startTime, period, offset);
 }
diff --git a/wpilibc/src/main/native/cpp/Timer.cpp b/wpilibc/src/main/native/cpp/Timer.cpp
index c91bc13..b428e03 100644
--- a/wpilibc/src/main/native/cpp/Timer.cpp
+++ b/wpilibc/src/main/native/cpp/Timer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,13 +7,7 @@
 
 #include "frc/Timer.h"
 
-#include <chrono>
-#include <thread>
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/DriverStation.h"
-#include "frc/RobotController.h"
+#include <units/time.h>
 
 namespace frc {
 
@@ -25,8 +19,6 @@
 
 using namespace frc;
 
-const double Timer::kRolloverTime = frc2::Timer::kRolloverTime.to<double>();
-
 Timer::Timer() { Reset(); }
 
 double Timer::Get() const { return m_timer.Get().to<double>(); }
diff --git a/wpilibc/src/main/native/cpp/Tracer.cpp b/wpilibc/src/main/native/cpp/Tracer.cpp
new file mode 100644
index 0000000..af5177a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/Tracer.cpp
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/Tracer.h"
+
+#include <wpi/Format.h>
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/DriverStation.h"
+
+using namespace frc;
+
+Tracer::Tracer() { ResetTimer(); }
+
+void Tracer::ResetTimer() { m_startTime = hal::fpga_clock::now(); }
+
+void Tracer::ClearEpochs() {
+  ResetTimer();
+  m_epochs.clear();
+}
+
+void Tracer::AddEpoch(wpi::StringRef epochName) {
+  auto currentTime = hal::fpga_clock::now();
+  m_epochs[epochName] = currentTime - m_startTime;
+  m_startTime = currentTime;
+}
+
+void Tracer::PrintEpochs() {
+  wpi::SmallString<128> buf;
+  wpi::raw_svector_ostream os(buf);
+  PrintEpochs(os);
+  if (!buf.empty()) DriverStation::ReportWarning(buf);
+}
+
+void Tracer::PrintEpochs(wpi::raw_ostream& os) {
+  using std::chrono::duration_cast;
+  using std::chrono::microseconds;
+
+  auto now = hal::fpga_clock::now();
+  if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
+    m_lastEpochsPrintTime = now;
+    for (const auto& epoch : m_epochs) {
+      os << '\t' << epoch.getKey() << ": "
+         << wpi::format(
+                "%.6f",
+                duration_cast<microseconds>(epoch.getValue()).count() / 1.0e6)
+         << "s\n";
+    }
+  }
+}
diff --git a/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
index fcd016e..dbd0d13 100644
--- a/wpilibc/src/main/native/cpp/Ultrasonic.cpp
+++ b/wpilibc/src/main/native/cpp/Ultrasonic.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,6 +9,7 @@
 
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/Base.h"
 #include "frc/Counter.h"
 #include "frc/DigitalInput.h"
 #include "frc/DigitalOutput.h"
@@ -177,8 +178,8 @@
 
 void Ultrasonic::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Ultrasonic");
-  builder.AddDoubleProperty("Value", [=]() { return GetRangeInches(); },
-                            nullptr);
+  builder.AddDoubleProperty(
+      "Value", [=]() { return GetRangeInches(); }, nullptr);
 }
 
 void Ultrasonic::Initialize() {
diff --git a/wpilibc/src/main/native/cpp/Utility.cpp b/wpilibc/src/main/native/cpp/Utility.cpp
index 11f6234..4b69cc8 100644
--- a/wpilibc/src/main/native/cpp/Utility.cpp
+++ b/wpilibc/src/main/native/cpp/Utility.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,19 +12,13 @@
 #include <execinfo.h>
 #endif
 
-#include <cstdio>
-#include <cstdlib>
-#include <cstring>
-
+#include <frc/Base.h>
 #include <hal/DriverStation.h>
-#include <hal/FRCUsageReporting.h>
 #include <wpi/Path.h>
 #include <wpi/SmallString.h>
 #include <wpi/StackTrace.h>
 #include <wpi/raw_ostream.h>
 
-#include "frc/ErrorBase.h"
-
 using namespace frc;
 
 bool wpi_assert_impl(bool conditionValue, const wpi::Twine& conditionText,
diff --git a/wpilibc/src/main/native/cpp/Watchdog.cpp b/wpilibc/src/main/native/cpp/Watchdog.cpp
index e5be4fa..81c7cc8 100644
--- a/wpilibc/src/main/native/cpp/Watchdog.cpp
+++ b/wpilibc/src/main/native/cpp/Watchdog.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 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,16 +7,24 @@
 
 #include "frc/Watchdog.h"
 
+#include <atomic>
+
+#include <hal/Notifier.h>
 #include <wpi/Format.h>
-#include <wpi/PriorityQueue.h>
+#include <wpi/SmallString.h>
+#include <wpi/priority_queue.h>
 #include <wpi/raw_ostream.h>
 
+#include "frc/DriverStation.h"
+#include "frc2/Timer.h"
+
 using namespace frc;
 
-constexpr std::chrono::milliseconds Watchdog::kMinPrintPeriod;
-
-class Watchdog::Thread : public wpi::SafeThread {
+class Watchdog::Impl {
  public:
+  Impl();
+  ~Impl();
+
   template <typename T>
   struct DerefGreater {
     constexpr bool operator()(const T& lhs, const T& rhs) const {
@@ -24,56 +32,96 @@
     }
   };
 
-  wpi::PriorityQueue<Watchdog*, std::vector<Watchdog*>, DerefGreater<Watchdog*>>
+  wpi::mutex m_mutex;
+  std::atomic<HAL_NotifierHandle> m_notifier;
+  wpi::priority_queue<Watchdog*, std::vector<Watchdog*>,
+                      DerefGreater<Watchdog*>>
       m_watchdogs;
 
+  void UpdateAlarm();
+
  private:
-  void Main() override;
+  void Main();
+
+  std::thread m_thread;
 };
 
-void Watchdog::Thread::Main() {
-  std::unique_lock lock(m_mutex);
+Watchdog::Impl::Impl() {
+  int32_t status = 0;
+  m_notifier = HAL_InitializeNotifier(&status);
+  wpi_setGlobalHALError(status);
+  HAL_SetNotifierName(m_notifier, "Watchdog", &status);
 
-  while (m_active) {
-    if (m_watchdogs.size() > 0) {
-      if (m_cond.wait_until(lock, m_watchdogs.top()->m_expirationTime) ==
-          std::cv_status::timeout) {
-        if (m_watchdogs.size() == 0 ||
-            m_watchdogs.top()->m_expirationTime > hal::fpga_clock::now()) {
-          continue;
-        }
+  m_thread = std::thread([=] { Main(); });
+}
 
-        // If the condition variable timed out, that means a Watchdog timeout
-        // has occurred, so call its timeout function.
-        auto watchdog = m_watchdogs.top();
-        m_watchdogs.pop();
+Watchdog::Impl::~Impl() {
+  int32_t status = 0;
+  // atomically set handle to 0, then clean
+  HAL_NotifierHandle handle = m_notifier.exchange(0);
+  HAL_StopNotifier(handle, &status);
+  wpi_setGlobalHALError(status);
 
-        auto now = hal::fpga_clock::now();
-        if (now - watchdog->m_lastTimeoutPrintTime > kMinPrintPeriod) {
-          watchdog->m_lastTimeoutPrintTime = now;
-          if (!watchdog->m_suppressTimeoutMessage) {
-            wpi::outs() << "Watchdog not fed within "
-                        << wpi::format("%.6f",
-                                       watchdog->m_timeout.count() / 1.0e9)
-                        << "s\n";
-          }
-        }
+  // Join the thread to ensure the handler has exited.
+  if (m_thread.joinable()) m_thread.join();
 
-        // Set expiration flag before calling the callback so any manipulation
-        // of the flag in the callback (e.g., calling Disable()) isn't
-        // clobbered.
-        watchdog->m_isExpired = true;
+  HAL_CleanNotifier(handle, &status);
+}
 
-        lock.unlock();
-        watchdog->m_callback();
-        lock.lock();
+void Watchdog::Impl::UpdateAlarm() {
+  int32_t status = 0;
+  // Return if we are being destructed, or were not created successfully
+  auto notifier = m_notifier.load();
+  if (notifier == 0) return;
+  if (m_watchdogs.empty())
+    HAL_CancelNotifierAlarm(notifier, &status);
+  else
+    HAL_UpdateNotifierAlarm(
+        notifier,
+        static_cast<uint64_t>(m_watchdogs.top()->m_expirationTime.to<double>() *
+                              1e6),
+        &status);
+  wpi_setGlobalHALError(status);
+}
+
+void Watchdog::Impl::Main() {
+  for (;;) {
+    int32_t status = 0;
+    HAL_NotifierHandle notifier = m_notifier.load();
+    if (notifier == 0) break;
+    uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
+    if (curTime == 0 || status != 0) break;
+
+    std::unique_lock lock(m_mutex);
+
+    if (m_watchdogs.empty()) continue;
+
+    // If the condition variable timed out, that means a Watchdog timeout
+    // has occurred, so call its timeout function.
+    auto watchdog = m_watchdogs.pop();
+
+    units::second_t now{curTime * 1e-6};
+    if (now - watchdog->m_lastTimeoutPrintTime > kMinPrintPeriod) {
+      watchdog->m_lastTimeoutPrintTime = now;
+      if (!watchdog->m_suppressTimeoutMessage) {
+        wpi::SmallString<128> buf;
+        wpi::raw_svector_ostream err(buf);
+        err << "Watchdog not fed within "
+            << wpi::format("%.6f", watchdog->m_timeout.to<double>()) << "s\n";
+        frc::DriverStation::ReportWarning(err.str());
       }
-      // Otherwise, a Watchdog removed itself from the queue (it notifies the
-      // scheduler of this) or a spurious wakeup occurred, so just rewait with
-      // the soonest watchdog timeout.
-    } else {
-      m_cond.wait(lock, [&] { return m_watchdogs.size() > 0 || !m_active; });
     }
+
+    // Set expiration flag before calling the callback so any manipulation
+    // of the flag in the callback (e.g., calling Disable()) isn't
+    // clobbered.
+    watchdog->m_isExpired = true;
+
+    lock.unlock();
+    watchdog->m_callback();
+    lock.lock();
+
+    UpdateAlarm();
   }
 }
 
@@ -81,12 +129,32 @@
     : Watchdog(units::second_t{timeout}, callback) {}
 
 Watchdog::Watchdog(units::second_t timeout, std::function<void()> callback)
-    : m_timeout(timeout), m_callback(callback), m_owner(&GetThreadOwner()) {}
+    : m_timeout(timeout), m_callback(callback), m_impl(GetImpl()) {}
 
 Watchdog::~Watchdog() { Disable(); }
 
+Watchdog::Watchdog(Watchdog&& rhs) { *this = std::move(rhs); }
+
+Watchdog& Watchdog::operator=(Watchdog&& rhs) {
+  m_impl = rhs.m_impl;
+  std::scoped_lock lock(m_impl->m_mutex);
+  m_startTime = rhs.m_startTime;
+  m_timeout = rhs.m_timeout;
+  m_expirationTime = rhs.m_expirationTime;
+  m_callback = std::move(rhs.m_callback);
+  m_lastTimeoutPrintTime = rhs.m_lastTimeoutPrintTime;
+  m_suppressTimeoutMessage = rhs.m_suppressTimeoutMessage;
+  m_tracer = std::move(rhs.m_tracer);
+  m_isExpired = rhs.m_isExpired;
+  if (m_expirationTime != 0_s) {
+    m_impl->m_watchdogs.remove(&rhs);
+    m_impl->m_watchdogs.emplace(this);
+  }
+  return *this;
+}
+
 double Watchdog::GetTime() const {
-  return (hal::fpga_clock::now() - m_startTime).count() / 1.0e6;
+  return (frc2::Timer::GetFPGATimestamp() - m_startTime).to<double>();
 }
 
 void Watchdog::SetTimeout(double timeout) {
@@ -94,100 +162,69 @@
 }
 
 void Watchdog::SetTimeout(units::second_t timeout) {
-  using std::chrono::duration_cast;
-  using std::chrono::microseconds;
+  m_startTime = frc2::Timer::GetFPGATimestamp();
+  m_tracer.ClearEpochs();
 
-  m_startTime = hal::fpga_clock::now();
-  m_epochs.clear();
-
-  // Locks mutex
-  auto thr = m_owner->GetThread();
-  if (!thr) return;
-
+  std::scoped_lock lock(m_impl->m_mutex);
   m_timeout = timeout;
   m_isExpired = false;
 
-  thr->m_watchdogs.remove(this);
-  m_expirationTime = m_startTime + duration_cast<microseconds>(m_timeout);
-  thr->m_watchdogs.emplace(this);
-  thr->m_cond.notify_all();
+  m_impl->m_watchdogs.remove(this);
+  m_expirationTime = m_startTime + m_timeout;
+  m_impl->m_watchdogs.emplace(this);
+  m_impl->UpdateAlarm();
 }
 
 double Watchdog::GetTimeout() const {
-  // Locks mutex
-  auto thr = m_owner->GetThread();
-
-  return m_timeout.count() / 1.0e9;
+  std::scoped_lock lock(m_impl->m_mutex);
+  return m_timeout.to<double>();
 }
 
 bool Watchdog::IsExpired() const {
-  // Locks mutex
-  auto thr = m_owner->GetThread();
-
+  std::scoped_lock lock(m_impl->m_mutex);
   return m_isExpired;
 }
 
 void Watchdog::AddEpoch(wpi::StringRef epochName) {
-  auto currentTime = hal::fpga_clock::now();
-  m_epochs[epochName] = currentTime - m_startTime;
-  m_startTime = currentTime;
+  m_tracer.AddEpoch(epochName);
 }
 
-void Watchdog::PrintEpochs() {
-  auto now = hal::fpga_clock::now();
-  if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
-    m_lastEpochsPrintTime = now;
-    for (const auto& epoch : m_epochs) {
-      wpi::outs() << '\t' << epoch.getKey() << ": "
-                  << wpi::format("%.6f", epoch.getValue().count() / 1.0e6)
-                  << "s\n";
-    }
-  }
-}
+void Watchdog::PrintEpochs() { m_tracer.PrintEpochs(); }
 
 void Watchdog::Reset() { Enable(); }
 
 void Watchdog::Enable() {
-  using std::chrono::duration_cast;
-  using std::chrono::microseconds;
+  m_startTime = frc2::Timer::GetFPGATimestamp();
+  m_tracer.ClearEpochs();
 
-  m_startTime = hal::fpga_clock::now();
-  m_epochs.clear();
-
-  // Locks mutex
-  auto thr = m_owner->GetThread();
-  if (!thr) return;
-
+  std::scoped_lock lock(m_impl->m_mutex);
   m_isExpired = false;
 
-  thr->m_watchdogs.remove(this);
-  m_expirationTime = m_startTime + duration_cast<microseconds>(m_timeout);
-  thr->m_watchdogs.emplace(this);
-  thr->m_cond.notify_all();
+  m_impl->m_watchdogs.remove(this);
+  m_expirationTime = m_startTime + m_timeout;
+  m_impl->m_watchdogs.emplace(this);
+  m_impl->UpdateAlarm();
 }
 
 void Watchdog::Disable() {
-  // Locks mutex
-  auto thr = m_owner->GetThread();
-  if (!thr) return;
+  std::scoped_lock lock(m_impl->m_mutex);
 
-  thr->m_watchdogs.remove(this);
-  thr->m_cond.notify_all();
+  if (m_expirationTime != 0_s) {
+    m_impl->m_watchdogs.remove(this);
+    m_expirationTime = 0_s;
+    m_impl->UpdateAlarm();
+  }
 }
 
 void Watchdog::SuppressTimeoutMessage(bool suppress) {
   m_suppressTimeoutMessage = suppress;
 }
 
-bool Watchdog::operator>(const Watchdog& rhs) {
+bool Watchdog::operator>(const Watchdog& rhs) const {
   return m_expirationTime > rhs.m_expirationTime;
 }
 
-wpi::SafeThreadOwner<Watchdog::Thread>& Watchdog::GetThreadOwner() {
-  static wpi::SafeThreadOwner<Thread> inst = [] {
-    wpi::SafeThreadOwner<Watchdog::Thread> inst;
-    inst.Start();
-    return inst;
-  }();
-  return inst;
+Watchdog::Impl* Watchdog::GetImpl() {
+  static Impl inst;
+  return &inst;
 }
diff --git a/wpilibc/src/main/native/cpp/XboxController.cpp b/wpilibc/src/main/native/cpp/XboxController.cpp
index b294257..de77624 100644
--- a/wpilibc/src/main/native/cpp/XboxController.cpp
+++ b/wpilibc/src/main/native/cpp/XboxController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 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.                                                               */
@@ -17,25 +17,25 @@
 
 double XboxController::GetX(JoystickHand hand) const {
   if (hand == kLeftHand) {
-    return GetRawAxis(0);
+    return GetRawAxis(static_cast<int>(Axis::kLeftX));
   } else {
-    return GetRawAxis(4);
+    return GetRawAxis(static_cast<int>(Axis::kRightX));
   }
 }
 
 double XboxController::GetY(JoystickHand hand) const {
   if (hand == kLeftHand) {
-    return GetRawAxis(1);
+    return GetRawAxis(static_cast<int>(Axis::kLeftY));
   } else {
-    return GetRawAxis(5);
+    return GetRawAxis(static_cast<int>(Axis::kRightY));
   }
 }
 
 double XboxController::GetTriggerAxis(JoystickHand hand) const {
   if (hand == kLeftHand) {
-    return GetRawAxis(2);
+    return GetRawAxis(static_cast<int>(Axis::kLeftTrigger));
   } else {
-    return GetRawAxis(3);
+    return GetRawAxis(static_cast<int>(Axis::kRightTrigger));
   }
 }
 
diff --git a/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp b/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
new file mode 100644
index 0000000..7482e22
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/HolonomicDriveController.h"
+
+#include <units/angular_velocity.h>
+
+using namespace frc;
+
+HolonomicDriveController::HolonomicDriveController(
+    const frc2::PIDController& xController,
+    const frc2::PIDController& yController,
+    const ProfiledPIDController<units::radian>& thetaController)
+    : m_xController(xController),
+      m_yController(yController),
+      m_thetaController(thetaController) {}
+
+bool HolonomicDriveController::AtReference() const {
+  const auto& eTranslate = m_poseError.Translation();
+  const auto& eRotate = m_poseError.Rotation();
+  const auto& tolTranslate = m_poseTolerance.Translation();
+  const auto& tolRotate = m_poseTolerance.Rotation();
+  return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
+         units::math::abs(eTranslate.Y()) < tolTranslate.Y() &&
+         units::math::abs(eRotate.Radians()) < tolRotate.Radians();
+}
+
+void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
+  m_poseTolerance = tolerance;
+}
+
+ChassisSpeeds HolonomicDriveController::Calculate(
+    const Pose2d& currentPose, const Pose2d& poseRef,
+    units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
+  // Calculate feedforward velocities (field-relative)
+  auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
+  auto yFF = linearVelocityRef * poseRef.Rotation().Sin();
+  auto thetaFF = units::radians_per_second_t(m_thetaController.Calculate(
+      currentPose.Rotation().Radians(), angleRef.Radians()));
+
+  m_poseError = poseRef.RelativeTo(currentPose);
+
+  if (!m_enabled) {
+    return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,
+                                                  currentPose.Rotation());
+  }
+
+  // Calculate feedback velocities (based on position error).
+  auto xFeedback = units::meters_per_second_t(m_xController.Calculate(
+      currentPose.X().to<double>(), poseRef.X().to<double>()));
+  auto yFeedback = units::meters_per_second_t(m_yController.Calculate(
+      currentPose.Y().to<double>(), poseRef.Y().to<double>()));
+
+  // Return next output.
+  return ChassisSpeeds::FromFieldRelativeSpeeds(
+      xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.Rotation());
+}
+
+ChassisSpeeds HolonomicDriveController::Calculate(
+    const Pose2d& currentPose, const Trajectory::State& desiredState,
+    const Rotation2d& angleRef) {
+  return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+                   angleRef);
+}
+
+void HolonomicDriveController::SetEnabled(bool enabled) { m_enabled = enabled; }
diff --git a/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
index 3abd4ba..7b9b623 100644
--- a/wpilibc/src/main/native/cpp/controller/PIDController.cpp
+++ b/wpilibc/src/main/native/cpp/controller/PIDController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,6 +12,7 @@
 
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/controller/ControllerUtil.h"
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
 
@@ -26,6 +27,12 @@
   frc::SendableRegistry::GetInstance().Add(this, "PIDController", instances);
 }
 
+void PIDController::SetPID(double Kp, double Ki, double Kd) {
+  m_Kp = Kp;
+  m_Ki = Ki;
+  m_Kd = Kd;
+}
+
 void PIDController::SetP(double Kp) { m_Kp = Kp; }
 
 void PIDController::SetI(double Ki) { m_Ki = Ki; }
@@ -42,13 +49,7 @@
   return units::second_t(m_period);
 }
 
-void PIDController::SetSetpoint(double setpoint) {
-  if (m_maximumInput > m_minimumInput) {
-    m_setpoint = std::clamp(setpoint, m_minimumInput, m_maximumInput);
-  } else {
-    m_setpoint = setpoint;
-  }
-}
+void PIDController::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
 
 double PIDController::GetSetpoint() const { return m_setpoint; }
 
@@ -60,11 +61,14 @@
 void PIDController::EnableContinuousInput(double minimumInput,
                                           double maximumInput) {
   m_continuous = true;
-  SetInputRange(minimumInput, maximumInput);
+  m_minimumInput = minimumInput;
+  m_maximumInput = maximumInput;
 }
 
 void PIDController::DisableContinuousInput() { m_continuous = false; }
 
+bool PIDController::IsContinuousInputEnabled() const { return m_continuous; }
+
 void PIDController::SetIntegratorRange(double minimumIntegral,
                                        double maximumIntegral) {
   m_minimumIntegral = minimumIntegral;
@@ -77,15 +81,20 @@
   m_velocityTolerance = velocityTolerance;
 }
 
-double PIDController::GetPositionError() const {
-  return GetContinuousError(m_positionError);
-}
+double PIDController::GetPositionError() const { return m_positionError; }
 
 double PIDController::GetVelocityError() const { return m_velocityError; }
 
 double PIDController::Calculate(double measurement) {
   m_prevError = m_positionError;
-  m_positionError = GetContinuousError(m_setpoint - measurement);
+
+  if (m_continuous) {
+    m_positionError = frc::GetModulusError<double>(
+        m_setpoint, measurement, m_minimumInput, m_maximumInput);
+  } else {
+    m_positionError = m_setpoint - measurement;
+  }
+
   m_velocityError = (m_positionError - m_prevError) / m_period.to<double>();
 
   if (m_Ki != 0) {
@@ -110,38 +119,13 @@
 
 void PIDController::InitSendable(frc::SendableBuilder& builder) {
   builder.SetSmartDashboardType("PIDController");
-  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("setpoint", [this] { return GetSetpoint(); },
-                            [this](double value) { SetSetpoint(value); });
-}
-
-double PIDController::GetContinuousError(double error) const {
-  if (m_continuous && m_inputRange > 0) {
-    error = std::fmod(error, m_inputRange);
-    if (std::abs(error) > m_inputRange / 2) {
-      if (error > 0) {
-        return error - m_inputRange;
-      } else {
-        return error + m_inputRange;
-      }
-    }
-  }
-
-  return error;
-}
-
-void PIDController::SetInputRange(double minimumInput, double maximumInput) {
-  m_minimumInput = minimumInput;
-  m_maximumInput = maximumInput;
-  m_inputRange = maximumInput - minimumInput;
-
-  // Clamp setpoint to new input range
-  if (m_maximumInput > m_minimumInput) {
-    m_setpoint = std::clamp(m_setpoint, m_minimumInput, m_maximumInput);
-  }
+  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(
+      "setpoint", [this] { return GetSetpoint(); },
+      [this](double value) { SetSetpoint(value); });
 }
diff --git a/wpilibc/src/main/native/cpp/controller/RamseteController.cpp b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
index c55c2e8..aad6937 100644
--- a/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
+++ b/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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,6 +9,8 @@
 
 #include <cmath>
 
+#include <units/math.h>
+
 using namespace frc;
 
 /**
@@ -45,11 +47,15 @@
     const Pose2d& currentPose, const Pose2d& poseRef,
     units::meters_per_second_t linearVelocityRef,
     units::radians_per_second_t angularVelocityRef) {
+  if (!m_enabled) {
+    return ChassisSpeeds{linearVelocityRef, 0_mps, angularVelocityRef};
+  }
+
   m_poseError = poseRef.RelativeTo(currentPose);
 
   // Aliases for equation readability
-  double eX = m_poseError.Translation().X().to<double>();
-  double eY = m_poseError.Translation().Y().to<double>();
+  double eX = m_poseError.X().to<double>();
+  double eY = m_poseError.Y().to<double>();
   double eTheta = m_poseError.Rotation().Radians().to<double>();
   double vRef = linearVelocityRef.to<double>();
   double omegaRef = angularVelocityRef.to<double>();
@@ -68,3 +74,5 @@
   return Calculate(currentPose, desiredState.pose, desiredState.velocity,
                    desiredState.velocity * desiredState.curvature);
 }
+
+void RamseteController::SetEnabled(bool enabled) { m_enabled = enabled; }
diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index 2d30a2b..ce9aa05 100644
--- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 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.                                                               */
@@ -216,9 +216,9 @@
   builder.SetSmartDashboardType("DifferentialDrive");
   builder.SetActuator(true);
   builder.SetSafeState([=] { StopMotor(); });
-  builder.AddDoubleProperty("Left Motor Speed",
-                            [=]() { return m_leftMotor->Get(); },
-                            [=](double value) { m_leftMotor->Set(value); });
+  builder.AddDoubleProperty(
+      "Left Motor Speed", [=]() { return m_leftMotor->Get(); },
+      [=](double value) { m_leftMotor->Set(value); });
   builder.AddDoubleProperty(
       "Right Motor Speed",
       [=]() { return m_rightMotor->Get() * m_rightSideInvertMultiplier; },
diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
index e03951b..983ce6a 100644
--- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 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.                                                               */
@@ -107,13 +107,13 @@
   builder.SetSmartDashboardType("KilloughDrive");
   builder.SetActuator(true);
   builder.SetSafeState([=] { StopMotor(); });
-  builder.AddDoubleProperty("Left Motor Speed",
-                            [=]() { return m_leftMotor->Get(); },
-                            [=](double value) { m_leftMotor->Set(value); });
-  builder.AddDoubleProperty("Right Motor Speed",
-                            [=]() { return m_rightMotor->Get(); },
-                            [=](double value) { m_rightMotor->Set(value); });
-  builder.AddDoubleProperty("Back Motor Speed",
-                            [=]() { return m_backMotor->Get(); },
-                            [=](double value) { m_backMotor->Set(value); });
+  builder.AddDoubleProperty(
+      "Left Motor Speed", [=]() { return m_leftMotor->Get(); },
+      [=](double value) { m_leftMotor->Set(value); });
+  builder.AddDoubleProperty(
+      "Right Motor Speed", [=]() { return m_rightMotor->Get(); },
+      [=](double value) { m_rightMotor->Set(value); });
+  builder.AddDoubleProperty(
+      "Back Motor Speed", [=]() { return m_backMotor->Get(); },
+      [=](double value) { m_backMotor->Set(value); });
 }
diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index 0102d6a..cb11d8a 100644
--- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -120,9 +120,9 @@
       [=](double value) {
         m_frontRightMotor->Set(value * m_rightSideInvertMultiplier);
       });
-  builder.AddDoubleProperty("Rear Left Motor Speed",
-                            [=]() { return m_rearLeftMotor->Get(); },
-                            [=](double value) { m_rearLeftMotor->Set(value); });
+  builder.AddDoubleProperty(
+      "Rear Left Motor Speed", [=]() { return m_rearLeftMotor->Get(); },
+      [=](double value) { m_rearLeftMotor->Set(value); });
   builder.AddDoubleProperty(
       "Rear Right Motor Speed",
       [=]() { return m_rearRightMotor->Get() * m_rightSideInvertMultiplier; },
diff --git a/wpilibc/src/main/native/cpp/frc2/Timer.cpp b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
index 76721bc..36da4c6 100644
--- a/wpilibc/src/main/native/cpp/frc2/Timer.cpp
+++ b/wpilibc/src/main/native/cpp/frc2/Timer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,8 +10,6 @@
 #include <chrono>
 #include <thread>
 
-#include <hal/FRCUsageReporting.h>
-
 #include "frc/DriverStation.h"
 #include "frc/RobotController.h"
 
@@ -36,9 +34,6 @@
 
 using namespace frc2;
 
-// for compatibility with msvc12--see C2864
-const units::second_t Timer::kRolloverTime = units::second_t((1ll << 32) / 1e6);
-
 Timer::Timer() { Reset(); }
 
 Timer::Timer(const Timer& rhs)
@@ -77,13 +72,6 @@
 
   std::scoped_lock lock(m_mutex);
   if (m_running) {
-    // If the current time is before the start time, then the FPGA clock rolled
-    // over. Compensate by adding the ~71 minutes that it takes to roll over to
-    // the current time.
-    if (currentTime < m_startTime) {
-      currentTime += kRolloverTime;
-    }
-
     result = (currentTime - m_startTime) + m_accumulatedTime;
   } else {
     result = m_accumulatedTime;
@@ -116,15 +104,22 @@
   }
 }
 
+bool Timer::HasElapsed(units::second_t period) const { return Get() > period; }
+
 bool Timer::HasPeriodPassed(units::second_t period) {
+  return AdvanceIfElapsed(period);
+}
+
+bool Timer::AdvanceIfElapsed(units::second_t period) {
   if (Get() > period) {
     std::scoped_lock lock(m_mutex);
     // Advance the start time by the period.
     m_startTime += period;
     // Don't set it to the current time... we want to avoid drift.
     return true;
+  } else {
+    return false;
   }
-  return false;
 }
 
 units::second_t Timer::GetFPGATimestamp() {
diff --git a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp b/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
deleted file mode 100644
index 1754830..0000000
--- a/wpilibc/src/main/native/cpp/geometry/Pose2d.cpp
+++ /dev/null
@@ -1,110 +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/geometry/Pose2d.h"
-
-#include <cmath>
-
-#include <wpi/json.h>
-
-using namespace frc;
-
-Pose2d::Pose2d(Translation2d translation, Rotation2d rotation)
-    : m_translation(translation), m_rotation(rotation) {}
-
-Pose2d::Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
-    : m_translation(x, y), m_rotation(rotation) {}
-
-Pose2d Pose2d::operator+(const Transform2d& other) const {
-  return TransformBy(other);
-}
-
-Pose2d& Pose2d::operator+=(const Transform2d& other) {
-  m_translation += other.Translation().RotateBy(m_rotation);
-  m_rotation += other.Rotation();
-  return *this;
-}
-
-Transform2d Pose2d::operator-(const Pose2d& other) const {
-  const auto pose = this->RelativeTo(other);
-  return Transform2d(pose.Translation(), pose.Rotation());
-}
-
-bool Pose2d::operator==(const Pose2d& other) const {
-  return m_translation == other.m_translation && m_rotation == other.m_rotation;
-}
-
-bool Pose2d::operator!=(const Pose2d& other) const {
-  return !operator==(other);
-}
-
-Pose2d Pose2d::TransformBy(const Transform2d& other) const {
-  return {m_translation + (other.Translation().RotateBy(m_rotation)),
-          m_rotation + other.Rotation()};
-}
-
-Pose2d Pose2d::RelativeTo(const Pose2d& other) const {
-  const Transform2d transform{other, *this};
-  return {transform.Translation(), transform.Rotation()};
-}
-
-Pose2d Pose2d::Exp(const Twist2d& twist) const {
-  const auto dx = twist.dx;
-  const auto dy = twist.dy;
-  const auto dtheta = twist.dtheta.to<double>();
-
-  const auto sinTheta = std::sin(dtheta);
-  const auto cosTheta = std::cos(dtheta);
-
-  double s, c;
-  if (std::abs(dtheta) < 1E-9) {
-    s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
-    c = 0.5 * dtheta;
-  } else {
-    s = sinTheta / dtheta;
-    c = (1 - cosTheta) / dtheta;
-  }
-
-  const Transform2d transform{Translation2d{dx * s - dy * c, dx * c + dy * s},
-                              Rotation2d{cosTheta, sinTheta}};
-
-  return *this + transform;
-}
-
-Twist2d Pose2d::Log(const Pose2d& end) const {
-  const auto transform = end.RelativeTo(*this);
-  const auto dtheta = transform.Rotation().Radians().to<double>();
-  const auto halfDtheta = dtheta / 2.0;
-
-  const auto cosMinusOne = transform.Rotation().Cos() - 1;
-
-  double halfThetaByTanOfHalfDtheta;
-
-  if (std::abs(cosMinusOne) < 1E-9) {
-    halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
-  } else {
-    halfThetaByTanOfHalfDtheta =
-        -(halfDtheta * transform.Rotation().Sin()) / cosMinusOne;
-  }
-
-  const Translation2d translationPart =
-      transform.Translation().RotateBy(
-          {halfThetaByTanOfHalfDtheta, -halfDtheta}) *
-      std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
-
-  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
deleted file mode 100644
index a3c4bea..0000000
--- a/wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp
+++ /dev/null
@@ -1,80 +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/geometry/Rotation2d.h"
-
-#include <cmath>
-
-#include <wpi/json.h>
-
-using namespace frc;
-
-Rotation2d::Rotation2d(units::radian_t value)
-    : m_value(value),
-      m_cos(units::math::cos(value)),
-      m_sin(units::math::sin(value)) {}
-
-Rotation2d::Rotation2d(double x, double y) {
-  const auto magnitude = std::hypot(x, y);
-  if (magnitude > 1e-6) {
-    m_sin = y / magnitude;
-    m_cos = x / magnitude;
-  } else {
-    m_sin = 0.0;
-    m_cos = 1.0;
-  }
-  m_value = units::radian_t(std::atan2(m_sin, m_cos));
-}
-
-Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
-  return RotateBy(other);
-}
-
-Rotation2d& Rotation2d::operator+=(const Rotation2d& other) {
-  double cos = Cos() * other.Cos() - Sin() * other.Sin();
-  double sin = Cos() * other.Sin() + Sin() * other.Cos();
-  m_cos = cos;
-  m_sin = sin;
-  m_value = units::radian_t(std::atan2(m_sin, m_cos));
-  return *this;
-}
-
-Rotation2d Rotation2d::operator-(const Rotation2d& other) const {
-  return *this + -other;
-}
-
-Rotation2d& Rotation2d::operator-=(const Rotation2d& other) {
-  *this += -other;
-  return *this;
-}
-
-Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); }
-
-Rotation2d Rotation2d::operator*(double scalar) const {
-  return Rotation2d(m_value * scalar);
-}
-
-bool Rotation2d::operator==(const Rotation2d& other) const {
-  return units::math::abs(m_value - other.m_value) < 1E-9_rad;
-}
-
-bool Rotation2d::operator!=(const Rotation2d& other) const {
-  return !operator==(other);
-}
-
-Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const {
-  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/Transform2d.cpp b/wpilibc/src/main/native/cpp/geometry/Transform2d.cpp
deleted file mode 100644
index 04f0419..0000000
--- a/wpilibc/src/main/native/cpp/geometry/Transform2d.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 "frc/geometry/Transform2d.h"
-
-#include "frc/geometry/Pose2d.h"
-
-using namespace frc;
-
-Transform2d::Transform2d(Pose2d initial, Pose2d final) {
-  // We are rotating the difference between the translations
-  // using a clockwise rotation matrix. This transforms the global
-  // delta into a local delta (relative to the initial pose).
-  m_translation = (final.Translation() - initial.Translation())
-                      .RotateBy(-initial.Rotation());
-
-  m_rotation = final.Rotation() - initial.Rotation();
-}
-
-Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
-    : m_translation(translation), m_rotation(rotation) {}
-
-bool Transform2d::operator==(const Transform2d& other) const {
-  return m_translation == other.m_translation && m_rotation == other.m_rotation;
-}
-
-bool Transform2d::operator!=(const Transform2d& other) const {
-  return !operator==(other);
-}
diff --git a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp b/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
deleted file mode 100644
index c3db7e3..0000000
--- a/wpilibc/src/main/native/cpp/geometry/Translation2d.cpp
+++ /dev/null
@@ -1,87 +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/geometry/Translation2d.h"
-
-#include <wpi/json.h>
-
-using namespace frc;
-
-Translation2d::Translation2d(units::meter_t x, units::meter_t y)
-    : m_x(x), m_y(y) {}
-
-units::meter_t Translation2d::Distance(const Translation2d& other) const {
-  return units::math::hypot(other.m_x - m_x, other.m_y - m_y);
-}
-
-units::meter_t Translation2d::Norm() const {
-  return units::math::hypot(m_x, m_y);
-}
-
-Translation2d Translation2d::RotateBy(const Rotation2d& other) const {
-  return {m_x * other.Cos() - m_y * other.Sin(),
-          m_x * other.Sin() + m_y * other.Cos()};
-}
-
-Translation2d Translation2d::operator+(const Translation2d& other) const {
-  return {X() + other.X(), Y() + other.Y()};
-}
-
-Translation2d& Translation2d::operator+=(const Translation2d& other) {
-  m_x += other.m_x;
-  m_y += other.m_y;
-  return *this;
-}
-
-Translation2d Translation2d::operator-(const Translation2d& other) const {
-  return *this + -other;
-}
-
-Translation2d& Translation2d::operator-=(const Translation2d& other) {
-  *this += -other;
-  return *this;
-}
-
-Translation2d Translation2d::operator-() const { return {-m_x, -m_y}; }
-
-Translation2d Translation2d::operator*(double scalar) const {
-  return {scalar * m_x, scalar * m_y};
-}
-
-Translation2d& Translation2d::operator*=(double scalar) {
-  m_x *= scalar;
-  m_y *= scalar;
-  return *this;
-}
-
-Translation2d Translation2d::operator/(double scalar) const {
-  return *this * (1.0 / scalar);
-}
-
-bool Translation2d::operator==(const Translation2d& other) const {
-  return units::math::abs(m_x - other.m_x) < 1E-9_m &&
-         units::math::abs(m_y - other.m_y) < 1E-9_m;
-}
-
-bool Translation2d::operator!=(const Translation2d& other) const {
-  return !operator==(other);
-}
-
-Translation2d& Translation2d::operator/=(double scalar) {
-  *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/DifferentialDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
deleted file mode 100644
index 07a8e3b..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/DifferentialDriveOdometry.h"
-
-#include <hal/FRCUsageReporting.h>
-
-using namespace frc;
-
-DifferentialDriveOdometry::DifferentialDriveOdometry(
-    const Rotation2d& gyroAngle, const Pose2d& initialPose)
-    : m_pose(initialPose) {
-  m_previousAngle = m_pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  HAL_Report(HALUsageReporting::kResourceType_Odometry,
-             HALUsageReporting::kOdometry_DifferentialDrive);
-}
-
-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;
-
-  m_prevLeftDistance = leftDistance;
-  m_prevRightDistance = rightDistance;
-
-  auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
-  auto angle = gyroAngle + m_gyroOffset;
-
-  auto newPose = m_pose.Exp(
-      {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()});
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}
diff --git a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
deleted file mode 100644
index 5b13f0e..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
+++ /dev/null
@@ -1,21 +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/DifferentialDriveWheelSpeeds.h"
-
-using namespace frc;
-
-void DifferentialDriveWheelSpeeds::Normalize(
-    units::meters_per_second_t attainableMaxSpeed) {
-  auto realMaxSpeed =
-      units::math::max(units::math::abs(left), units::math::abs(right));
-
-  if (realMaxSpeed > attainableMaxSpeed) {
-    left = left / realMaxSpeed * attainableMaxSpeed;
-    right = right / realMaxSpeed * attainableMaxSpeed;
-  }
-}
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
deleted file mode 100644
index cf6ebb5..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/kinematics/MecanumDriveKinematics.h"
-
-using namespace frc;
-
-MecanumDriveWheelSpeeds MecanumDriveKinematics::ToWheelSpeeds(
-    const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
-  // We have a new center of rotation. We need to compute the matrix again.
-  if (centerOfRotation != m_previousCoR) {
-    auto fl = m_frontLeftWheel - centerOfRotation;
-    auto fr = m_frontRightWheel - centerOfRotation;
-    auto rl = m_rearLeftWheel - centerOfRotation;
-    auto rr = m_rearRightWheel - centerOfRotation;
-
-    SetInverseKinematics(fl, fr, rl, rr);
-
-    m_previousCoR = centerOfRotation;
-  }
-
-  Eigen::Vector3d chassisSpeedsVector;
-  chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
-      chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
-
-  Eigen::Matrix<double, 4, 1> wheelsMatrix =
-      m_inverseKinematics * chassisSpeedsVector;
-
-  MecanumDriveWheelSpeeds wheelSpeeds;
-  wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsMatrix(0, 0)};
-  wheelSpeeds.frontRight = units::meters_per_second_t{wheelsMatrix(1, 0)};
-  wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsMatrix(2, 0)};
-  wheelSpeeds.rearRight = units::meters_per_second_t{wheelsMatrix(3, 0)};
-  return wheelSpeeds;
-}
-
-ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
-    const MecanumDriveWheelSpeeds& wheelSpeeds) {
-  Eigen::Matrix<double, 4, 1> wheelSpeedsMatrix;
-  // clang-format off
-  wheelSpeedsMatrix << wheelSpeeds.frontLeft.to<double>(), wheelSpeeds.frontRight.to<double>(),
-                       wheelSpeeds.rearLeft.to<double>(), wheelSpeeds.rearRight.to<double>();
-  // clang-format on
-
-  Eigen::Vector3d chassisSpeedsVector =
-      m_forwardKinematics.solve(wheelSpeedsMatrix);
-
-  return {units::meters_per_second_t{chassisSpeedsVector(0)},
-          units::meters_per_second_t{chassisSpeedsVector(1)},
-          units::radians_per_second_t{chassisSpeedsVector(2)}};
-}
-
-void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,
-                                                  Translation2d fr,
-                                                  Translation2d rl,
-                                                  Translation2d rr) {
-  // clang-format off
-  m_inverseKinematics << 1, -1, (-(fl.X() + fl.Y())).template to<double>(),
-                         1,  1, (fr.X() - fr.Y()).template to<double>(),
-                         1,  1, (rl.X() - rl.Y()).template to<double>(),
-                         1, -1, (-(rr.X() + rr.Y())).template to<double>();
-  // clang-format on
-  m_inverseKinematics /= std::sqrt(2);
-}
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
deleted file mode 100644
index 3843483..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ /dev/null
@@ -1,43 +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/MecanumDriveOdometry.h"
-
-#include <hal/FRCUsageReporting.h>
-
-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;
-  HAL_Report(HALUsageReporting::kResourceType_Odometry,
-             HALUsageReporting::kOdometry_MecanumDrive);
-}
-
-const Pose2d& MecanumDriveOdometry::UpdateWithTime(
-    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);
-
-  auto newPose = m_pose.Exp(
-      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}
diff --git a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
deleted file mode 100644
index 1641ba8..0000000
--- a/wpilibc/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
+++ /dev/null
@@ -1,34 +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/MecanumDriveWheelSpeeds.h"
-
-#include <algorithm>
-#include <array>
-#include <cmath>
-
-using namespace frc;
-
-void MecanumDriveWheelSpeeds::Normalize(
-    units::meters_per_second_t attainableMaxSpeed) {
-  std::array<units::meters_per_second_t, 4> wheelSpeeds{frontLeft, frontRight,
-                                                        rearLeft, rearRight};
-  units::meters_per_second_t realMaxSpeed = *std::max_element(
-      wheelSpeeds.begin(), wheelSpeeds.end(), [](const auto& a, const auto& b) {
-        return units::math::abs(a) < units::math::abs(b);
-      });
-
-  if (realMaxSpeed > attainableMaxSpeed) {
-    for (int i = 0; i < 4; ++i) {
-      wheelSpeeds[i] = wheelSpeeds[i] / realMaxSpeed * attainableMaxSpeed;
-    }
-    frontLeft = wheelSpeeds[0];
-    frontRight = wheelSpeeds[1];
-    rearLeft = wheelSpeeds[2];
-    rearRight = wheelSpeeds[3];
-  }
-}
diff --git a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
index 10996e4..517ecf7 100644
--- a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2012-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2012-2020 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.                                                               */
@@ -104,10 +104,12 @@
   // Force table generation now to make sure everything is defined
   UpdateValuesUnsafe();
   if (enabled) {
+    if (this->enabled) this->enabled();
   } else {
     m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
       cbdata.builder.StopLiveWindowMode();
     });
+    if (this->disabled) this->disabled();
   }
   m_impl->enabledEntry.SetBoolean(enabled);
 }
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
index b6f8ce9..663cc19 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -30,7 +30,7 @@
 }  // namespace detail
 
 void SendableCameraWrapper::InitSendable(SendableBuilder& builder) {
-  builder.AddStringProperty(".ShuffleboardURI", [this] { return m_uri; },
-                            nullptr);
+  builder.AddStringProperty(
+      ".ShuffleboardURI", [this] { return m_uri; }, nullptr);
 }
 }  // namespace frc
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
index bb9dc9e..d73f635 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 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.                                                               */
@@ -47,7 +47,7 @@
   wpi::SmallVector<char, 16> storage;
   auto titleRef = title.toStringRef(storage);
   if (m_layouts.count(titleRef) == 0) {
-    auto layout = std::make_unique<ShuffleboardLayout>(*this, type, titleRef);
+    auto layout = std::make_unique<ShuffleboardLayout>(*this, titleRef, type);
     auto ptr = layout.get();
     m_components.emplace_back(std::move(layout));
     m_layouts.insert(std::make_pair(titleRef, ptr));
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
index 9717a8e..3316b3e 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 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,6 +10,7 @@
 #include <hal/FRCUsageReporting.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
+#include <wpi/SmallVector.h>
 #include <wpi/StringMap.h>
 
 #include "frc/shuffleboard/Shuffleboard.h"
@@ -43,7 +44,7 @@
 
 void ShuffleboardInstance::Update() {
   if (m_impl->tabsChanged) {
-    std::vector<std::string> tabTitles;
+    wpi::SmallVector<std::string, 16> tabTitles;
     for (auto& entry : m_impl->tabs) {
       tabTitles.emplace_back(entry.second.GetTitle());
     }
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp
index 8418b77..1cbfb80 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 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,11 +10,11 @@
 using namespace frc;
 
 ShuffleboardLayout::ShuffleboardLayout(ShuffleboardContainer& parent,
-                                       const wpi::Twine& name,
+                                       const wpi::Twine& title,
                                        const wpi::Twine& type)
-    : ShuffleboardValue(type),
-      ShuffleboardComponent(parent, type, name),
-      ShuffleboardContainer(name) {
+    : ShuffleboardValue(title),
+      ShuffleboardComponent(parent, title, type),
+      ShuffleboardContainer(title) {
   m_isLayout = true;
 }
 
diff --git a/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp
new file mode 100644
index 0000000..74a5180
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/ADXRS450_GyroSim.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/ADXRS450_Gyro.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+ADXRS450_GyroSim::ADXRS450_GyroSim(const frc::ADXRS450_Gyro& gyro) {
+  wpi::SmallString<128> fullname;
+  wpi::raw_svector_ostream os(fullname);
+  os << "ADXRS450_Gyro" << '[' << gyro.GetPort() << ']';
+  frc::sim::SimDeviceSim deviceSim{fullname.c_str()};
+  m_simAngle = deviceSim.GetDouble("Angle");
+  m_simRate = deviceSim.GetDouble("Rate");
+}
+
+void ADXRS450_GyroSim::SetAngle(units::degree_t angle) {
+  m_simAngle.Set(angle.to<double>());
+}
+
+void ADXRS450_GyroSim::SetRate(units::degrees_per_second_t rate) {
+  m_simRate.Set(rate.to<double>());
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp b/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp
new file mode 100644
index 0000000..743c51d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp
@@ -0,0 +1,119 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/AddressableLEDSim.h"
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include <hal/simulation/AddressableLEDData.h>
+
+using namespace frc;
+using namespace frc::sim;
+
+AddressableLEDSim::AddressableLEDSim() : m_index{0} {}
+
+AddressableLEDSim::AddressableLEDSim(const AddressableLED& addressableLED)
+    : m_index{0} {}
+
+AddressableLEDSim AddressableLEDSim::CreateForChannel(int pwmChannel) {
+  int index = HALSIM_FindAddressableLEDForChannel(pwmChannel);
+  if (index < 0)
+    throw std::out_of_range("no addressable LED found for PWM channel");
+  return AddressableLEDSim{index};
+}
+
+AddressableLEDSim AddressableLEDSim::CreateForIndex(int index) {
+  return AddressableLEDSim{index};
+}
+
+std::unique_ptr<CallbackStore> AddressableLEDSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAddressableLEDInitializedCallback);
+  store->SetUid(HALSIM_RegisterAddressableLEDInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AddressableLEDSim::GetInitialized() const {
+  return HALSIM_GetAddressableLEDInitialized(m_index);
+}
+
+void AddressableLEDSim::SetInitialized(bool initialized) {
+  HALSIM_SetAddressableLEDInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> AddressableLEDSim::RegisterOutputPortCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAddressableLEDOutputPortCallback);
+  store->SetUid(HALSIM_RegisterAddressableLEDOutputPortCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AddressableLEDSim::GetOutputPort() const {
+  return HALSIM_GetAddressableLEDOutputPort(m_index);
+}
+
+void AddressableLEDSim::SetOutputPort(int outputPort) {
+  HALSIM_SetAddressableLEDOutputPort(m_index, outputPort);
+}
+
+std::unique_ptr<CallbackStore> AddressableLEDSim::RegisterLengthCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAddressableLEDLengthCallback);
+  store->SetUid(HALSIM_RegisterAddressableLEDLengthCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AddressableLEDSim::GetLength() const {
+  return HALSIM_GetAddressableLEDLength(m_index);
+}
+
+void AddressableLEDSim::SetLength(int length) {
+  HALSIM_SetAddressableLEDLength(m_index, length);
+}
+
+std::unique_ptr<CallbackStore> AddressableLEDSim::RegisterRunningCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAddressableLEDRunningCallback);
+  store->SetUid(HALSIM_RegisterAddressableLEDRunningCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AddressableLEDSim::GetRunning() const {
+  return HALSIM_GetAddressableLEDRunning(m_index);
+}
+
+void AddressableLEDSim::SetRunning(bool running) {
+  HALSIM_SetAddressableLEDRunning(m_index, running);
+}
+
+std::unique_ptr<CallbackStore> AddressableLEDSim::RegisterDataCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAddressableLEDDataCallback);
+  store->SetUid(HALSIM_RegisterAddressableLEDDataCallback(
+      m_index, &ConstBufferCallbackStoreThunk, store.get()));
+  return store;
+}
+
+int AddressableLEDSim::GetData(struct HAL_AddressableLEDData* data) const {
+  return HALSIM_GetAddressableLEDData(m_index, data);
+}
+
+void AddressableLEDSim::SetData(struct HAL_AddressableLEDData* data,
+                                int length) {
+  HALSIM_SetAddressableLEDData(m_index, data, length);
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp
new file mode 100644
index 0000000..5fee737
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp
@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/AnalogEncoderSim.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/AnalogEncoder.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+AnalogEncoderSim::AnalogEncoderSim(const frc::AnalogEncoder& encoder) {
+  wpi::SmallString<128> fullname;
+  wpi::raw_svector_ostream os(fullname);
+  os << "AnalogEncoder" << '[' << encoder.GetChannel() << ']';
+  frc::sim::SimDeviceSim deviceSim{fullname.c_str()};
+  m_positionSim = deviceSim.GetDouble("Position");
+}
+
+void AnalogEncoderSim::SetPosition(frc::Rotation2d angle) {
+  SetTurns(angle.Degrees());
+}
+
+void AnalogEncoderSim::SetTurns(units::turn_t turns) {
+  m_positionSim.Set(turns.to<double>());
+}
+
+units::turn_t AnalogEncoderSim::GetTurns() {
+  return units::turn_t{m_positionSim.Get()};
+}
+
+frc::Rotation2d AnalogEncoderSim::GetPosition() {
+  units::radian_t rads = GetTurns();
+  return frc::Rotation2d{rads};
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp
new file mode 100644
index 0000000..b7cab6b
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/AnalogGyroSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/AnalogGyroData.h>
+
+#include "frc/AnalogGyro.h"
+#include "frc/AnalogInput.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+AnalogGyroSim::AnalogGyroSim(const AnalogGyro& gyro)
+    : m_index{gyro.GetAnalogInput()->GetChannel()} {}
+
+AnalogGyroSim::AnalogGyroSim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> AnalogGyroSim::RegisterAngleCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogGyroAngleCallback);
+  store->SetUid(HALSIM_RegisterAnalogGyroAngleCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogGyroSim::GetAngle() const {
+  return HALSIM_GetAnalogGyroAngle(m_index);
+}
+
+void AnalogGyroSim::SetAngle(double angle) {
+  HALSIM_SetAnalogGyroAngle(m_index, angle);
+}
+
+std::unique_ptr<CallbackStore> AnalogGyroSim::RegisterRateCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogGyroRateCallback);
+  store->SetUid(HALSIM_RegisterAnalogGyroRateCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogGyroSim::GetRate() const {
+  return HALSIM_GetAnalogGyroRate(m_index);
+}
+
+void AnalogGyroSim::SetRate(double rate) {
+  HALSIM_SetAnalogGyroRate(m_index, rate);
+}
+
+std::unique_ptr<CallbackStore> AnalogGyroSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogGyroInitializedCallback);
+  store->SetUid(HALSIM_RegisterAnalogGyroInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AnalogGyroSim::GetInitialized() const {
+  return HALSIM_GetAnalogGyroInitialized(m_index);
+}
+
+void AnalogGyroSim::SetInitialized(bool initialized) {
+  HALSIM_SetAnalogGyroInitialized(m_index, initialized);
+}
+
+void AnalogGyroSim::ResetData() { HALSIM_ResetAnalogGyroData(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp
new file mode 100644
index 0000000..057a188
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp
@@ -0,0 +1,182 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/AnalogInputSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/AnalogInData.h>
+
+#include "frc/AnalogInput.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+AnalogInputSim::AnalogInputSim(const AnalogInput& analogInput)
+    : m_index{analogInput.GetChannel()} {}
+
+AnalogInputSim::AnalogInputSim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInInitializedCallback);
+  store->SetUid(HALSIM_RegisterAnalogInInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AnalogInputSim::GetInitialized() const {
+  return HALSIM_GetAnalogInInitialized(m_index);
+}
+
+void AnalogInputSim::SetInitialized(bool initialized) {
+  HALSIM_SetAnalogInInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterAverageBitsCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInAverageBitsCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAverageBitsCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AnalogInputSim::GetAverageBits() const {
+  return HALSIM_GetAnalogInAverageBits(m_index);
+}
+
+void AnalogInputSim::SetAverageBits(int averageBits) {
+  HALSIM_SetAnalogInAverageBits(m_index, averageBits);
+}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterOversampleBitsCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInOversampleBitsCallback);
+  store->SetUid(HALSIM_RegisterAnalogInOversampleBitsCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AnalogInputSim::GetOversampleBits() const {
+  return HALSIM_GetAnalogInOversampleBits(m_index);
+}
+
+void AnalogInputSim::SetOversampleBits(int oversampleBits) {
+  HALSIM_SetAnalogInOversampleBits(m_index, oversampleBits);
+}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterVoltageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInVoltageCallback);
+  store->SetUid(HALSIM_RegisterAnalogInVoltageCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogInputSim::GetVoltage() const {
+  return HALSIM_GetAnalogInVoltage(m_index);
+}
+
+void AnalogInputSim::SetVoltage(double voltage) {
+  HALSIM_SetAnalogInVoltage(m_index, voltage);
+}
+
+std::unique_ptr<CallbackStore>
+AnalogInputSim::RegisterAccumulatorInitializedCallback(NotifyCallback callback,
+                                                       bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback,
+      &HALSIM_CancelAnalogInAccumulatorInitializedCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAccumulatorInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AnalogInputSim::GetAccumulatorInitialized() const {
+  return HALSIM_GetAnalogInAccumulatorInitialized(m_index);
+}
+
+void AnalogInputSim::SetAccumulatorInitialized(bool accumulatorInitialized) {
+  HALSIM_SetAnalogInAccumulatorInitialized(m_index, accumulatorInitialized);
+}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterAccumulatorValueCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorValueCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAccumulatorValueCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int64_t AnalogInputSim::GetAccumulatorValue() const {
+  return HALSIM_GetAnalogInAccumulatorValue(m_index);
+}
+
+void AnalogInputSim::SetAccumulatorValue(int64_t accumulatorValue) {
+  HALSIM_SetAnalogInAccumulatorValue(m_index, accumulatorValue);
+}
+
+std::unique_ptr<CallbackStore> AnalogInputSim::RegisterAccumulatorCountCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorCountCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAccumulatorCountCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int64_t AnalogInputSim::GetAccumulatorCount() const {
+  return HALSIM_GetAnalogInAccumulatorCount(m_index);
+}
+
+void AnalogInputSim::SetAccumulatorCount(int64_t accumulatorCount) {
+  HALSIM_SetAnalogInAccumulatorCount(m_index, accumulatorCount);
+}
+
+std::unique_ptr<CallbackStore>
+AnalogInputSim::RegisterAccumulatorCenterCallback(NotifyCallback callback,
+                                                  bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorCenterCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAccumulatorCenterCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AnalogInputSim::GetAccumulatorCenter() const {
+  return HALSIM_GetAnalogInAccumulatorCenter(m_index);
+}
+
+void AnalogInputSim::SetAccumulatorCenter(int accumulatorCenter) {
+  HALSIM_SetAnalogInAccumulatorCenter(m_index, accumulatorCenter);
+}
+
+std::unique_ptr<CallbackStore>
+AnalogInputSim::RegisterAccumulatorDeadbandCallback(NotifyCallback callback,
+                                                    bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogInAccumulatorDeadbandCallback);
+  store->SetUid(HALSIM_RegisterAnalogInAccumulatorDeadbandCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int AnalogInputSim::GetAccumulatorDeadband() const {
+  return HALSIM_GetAnalogInAccumulatorDeadband(m_index);
+}
+
+void AnalogInputSim::SetAccumulatorDeadband(int accumulatorDeadband) {
+  HALSIM_SetAnalogInAccumulatorDeadband(m_index, accumulatorDeadband);
+}
+
+void AnalogInputSim::ResetData() { HALSIM_ResetAnalogInData(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp
new file mode 100644
index 0000000..4de9082
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/AnalogOutputSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/AnalogOutData.h>
+
+#include "frc/AnalogOutput.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+AnalogOutputSim::AnalogOutputSim(const AnalogOutput& analogOutput)
+    : m_index{analogOutput.GetChannel()} {}
+
+AnalogOutputSim::AnalogOutputSim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> AnalogOutputSim::RegisterVoltageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogOutVoltageCallback);
+  store->SetUid(HALSIM_RegisterAnalogOutVoltageCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogOutputSim::GetVoltage() const {
+  return HALSIM_GetAnalogOutVoltage(m_index);
+}
+
+void AnalogOutputSim::SetVoltage(double voltage) {
+  HALSIM_SetAnalogOutVoltage(m_index, voltage);
+}
+
+std::unique_ptr<CallbackStore> AnalogOutputSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogOutInitializedCallback);
+  store->SetUid(HALSIM_RegisterAnalogOutInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AnalogOutputSim::GetInitialized() const {
+  return HALSIM_GetAnalogOutInitialized(m_index);
+}
+
+void AnalogOutputSim::SetInitialized(bool initialized) {
+  HALSIM_SetAnalogOutInitialized(m_index, initialized);
+}
+
+void AnalogOutputSim::ResetData() { HALSIM_ResetAnalogOutData(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp
new file mode 100644
index 0000000..3325827
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/AnalogTriggerSim.h"
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include <hal/simulation/AnalogTriggerData.h>
+
+#include "frc/AnalogTrigger.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+AnalogTriggerSim::AnalogTriggerSim(const AnalogTrigger& analogTrigger)
+    : m_index{analogTrigger.GetIndex()} {}
+
+AnalogTriggerSim AnalogTriggerSim::CreateForChannel(int channel) {
+  int index = HALSIM_FindAnalogTriggerForChannel(channel);
+  if (index < 0) throw std::out_of_range("no analog trigger found for channel");
+  return AnalogTriggerSim{index};
+}
+
+AnalogTriggerSim AnalogTriggerSim::CreateForIndex(int index) {
+  return AnalogTriggerSim{index};
+}
+
+std::unique_ptr<CallbackStore> AnalogTriggerSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAnalogTriggerInitializedCallback);
+  store->SetUid(HALSIM_RegisterAnalogTriggerInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool AnalogTriggerSim::GetInitialized() const {
+  return HALSIM_GetAnalogTriggerInitialized(m_index);
+}
+
+void AnalogTriggerSim::SetInitialized(bool initialized) {
+  HALSIM_SetAnalogTriggerInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore>
+AnalogTriggerSim::RegisterTriggerLowerBoundCallback(NotifyCallback callback,
+                                                    bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback,
+      &HALSIM_CancelAnalogTriggerTriggerLowerBoundCallback);
+  store->SetUid(HALSIM_RegisterAnalogTriggerTriggerLowerBoundCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogTriggerSim::GetTriggerLowerBound() const {
+  return HALSIM_GetAnalogTriggerTriggerLowerBound(m_index);
+}
+
+void AnalogTriggerSim::SetTriggerLowerBound(double triggerLowerBound) {
+  HALSIM_SetAnalogTriggerTriggerLowerBound(m_index, triggerLowerBound);
+}
+
+std::unique_ptr<CallbackStore>
+AnalogTriggerSim::RegisterTriggerUpperBoundCallback(NotifyCallback callback,
+                                                    bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback,
+      &HALSIM_CancelAnalogTriggerTriggerUpperBoundCallback);
+  store->SetUid(HALSIM_RegisterAnalogTriggerTriggerUpperBoundCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double AnalogTriggerSim::GetTriggerUpperBound() const {
+  return HALSIM_GetAnalogTriggerTriggerUpperBound(m_index);
+}
+
+void AnalogTriggerSim::SetTriggerUpperBound(double triggerUpperBound) {
+  HALSIM_SetAnalogTriggerTriggerUpperBound(m_index, triggerUpperBound);
+}
+
+void AnalogTriggerSim::ResetData() { HALSIM_ResetAnalogTriggerData(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp b/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp
new file mode 100644
index 0000000..601ee6e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp
@@ -0,0 +1,112 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/BuiltInAccelerometerSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/AccelerometerData.h>
+
+#include "frc/BuiltInAccelerometer.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+BuiltInAccelerometerSim::BuiltInAccelerometerSim() : m_index{0} {}
+
+BuiltInAccelerometerSim::BuiltInAccelerometerSim(const BuiltInAccelerometer&)
+    : m_index{0} {}
+
+std::unique_ptr<CallbackStore> BuiltInAccelerometerSim::RegisterActiveCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAccelerometerActiveCallback);
+  store->SetUid(HALSIM_RegisterAccelerometerActiveCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool BuiltInAccelerometerSim::GetActive() const {
+  return HALSIM_GetAccelerometerActive(m_index);
+}
+
+void BuiltInAccelerometerSim::SetActive(bool active) {
+  HALSIM_SetAccelerometerActive(m_index, active);
+}
+
+std::unique_ptr<CallbackStore> BuiltInAccelerometerSim::RegisterRangeCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAccelerometerRangeCallback);
+  store->SetUid(HALSIM_RegisterAccelerometerRangeCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+HAL_AccelerometerRange BuiltInAccelerometerSim::GetRange() const {
+  return HALSIM_GetAccelerometerRange(m_index);
+}
+
+void BuiltInAccelerometerSim::SetRange(HAL_AccelerometerRange range) {
+  HALSIM_SetAccelerometerRange(m_index, range);
+}
+
+std::unique_ptr<CallbackStore> BuiltInAccelerometerSim::RegisterXCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAccelerometerXCallback);
+  store->SetUid(HALSIM_RegisterAccelerometerXCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double BuiltInAccelerometerSim::GetX() const {
+  return HALSIM_GetAccelerometerX(m_index);
+}
+
+void BuiltInAccelerometerSim::SetX(double x) {
+  HALSIM_SetAccelerometerX(m_index, x);
+}
+
+std::unique_ptr<CallbackStore> BuiltInAccelerometerSim::RegisterYCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAccelerometerYCallback);
+  store->SetUid(HALSIM_RegisterAccelerometerYCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double BuiltInAccelerometerSim::GetY() const {
+  return HALSIM_GetAccelerometerY(m_index);
+}
+
+void BuiltInAccelerometerSim::SetY(double y) {
+  HALSIM_SetAccelerometerY(m_index, y);
+}
+
+std::unique_ptr<CallbackStore> BuiltInAccelerometerSim::RegisterZCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelAccelerometerZCallback);
+  store->SetUid(HALSIM_RegisterAccelerometerZCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double BuiltInAccelerometerSim::GetZ() const {
+  return HALSIM_GetAccelerometerZ(m_index);
+}
+
+void BuiltInAccelerometerSim::SetZ(double z) {
+  HALSIM_SetAccelerometerZ(m_index, z);
+}
+
+void BuiltInAccelerometerSim::ResetData() {
+  HALSIM_ResetAccelerometerData(m_index);
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp b/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp
new file mode 100644
index 0000000..5c38372
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+void frc::sim::CallbackStoreThunk(const char* name, void* param,
+                                  const HAL_Value* value) {
+  reinterpret_cast<CallbackStore*>(param)->callback(name, value);
+}
+
+void frc::sim::ConstBufferCallbackStoreThunk(const char* name, void* param,
+                                             const unsigned char* buffer,
+                                             unsigned int count) {
+  reinterpret_cast<CallbackStore*>(param)->constBufferCallback(name, buffer,
+                                                               count);
+}
+
+CallbackStore::CallbackStore(int32_t i, NotifyCallback cb,
+                             CancelCallbackNoIndexFunc ccf)
+    : index(i), callback(cb), cancelType(NoIndex) {
+  this->ccnif = ccf;
+}
+
+CallbackStore::CallbackStore(int32_t i, int32_t u, NotifyCallback cb,
+                             CancelCallbackFunc ccf)
+    : index(i), uid(u), callback(cb), cancelType(Normal) {
+  this->ccf = ccf;
+}
+
+CallbackStore::CallbackStore(int32_t i, int32_t c, int32_t u, NotifyCallback cb,
+                             CancelCallbackChannelFunc ccf)
+    : index(i), channel(c), uid(u), callback(cb), cancelType(Channel) {
+  this->cccf = ccf;
+}
+
+CallbackStore::CallbackStore(int32_t i, ConstBufferCallback cb,
+                             CancelCallbackNoIndexFunc ccf)
+    : index(i), constBufferCallback(cb), cancelType(NoIndex) {
+  this->ccnif = ccf;
+}
+
+CallbackStore::CallbackStore(int32_t i, int32_t u, ConstBufferCallback cb,
+                             CancelCallbackFunc ccf)
+    : index(i), uid(u), constBufferCallback(cb), cancelType(Normal) {
+  this->ccf = ccf;
+}
+
+CallbackStore::CallbackStore(int32_t i, int32_t c, int32_t u,
+                             ConstBufferCallback cb,
+                             CancelCallbackChannelFunc ccf)
+    : index(i),
+      channel(c),
+      uid(u),
+      constBufferCallback(cb),
+      cancelType(Channel) {
+  this->cccf = ccf;
+}
+
+CallbackStore::~CallbackStore() {
+  switch (cancelType) {
+    case Normal:
+      ccf(index, uid);
+      break;
+    case Channel:
+      cccf(index, channel, uid);
+      break;
+    case NoIndex:
+      ccnif(uid);
+      break;
+  }
+}
+
+void CallbackStore::SetUid(int32_t uid) { this->uid = uid; }
diff --git a/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp b/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp
new file mode 100644
index 0000000..9330735
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/DIOSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/DIOData.h>
+
+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DIOSim::DIOSim(const DigitalInput& input) : m_index{input.GetChannel()} {}
+
+DIOSim::DIOSim(const DigitalOutput& output) : m_index{output.GetChannel()} {}
+
+DIOSim::DIOSim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> DIOSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDIOInitializedCallback);
+  store->SetUid(HALSIM_RegisterDIOInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DIOSim::GetInitialized() const {
+  return HALSIM_GetDIOInitialized(m_index);
+}
+
+void DIOSim::SetInitialized(bool initialized) {
+  HALSIM_SetDIOInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> DIOSim::RegisterValueCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(m_index, -1, callback,
+                                               &HALSIM_CancelDIOValueCallback);
+  store->SetUid(HALSIM_RegisterDIOValueCallback(m_index, &CallbackStoreThunk,
+                                                store.get(), initialNotify));
+  return store;
+}
+
+bool DIOSim::GetValue() const { return HALSIM_GetDIOValue(m_index); }
+
+void DIOSim::SetValue(bool value) { HALSIM_SetDIOValue(m_index, value); }
+
+std::unique_ptr<CallbackStore> DIOSim::RegisterPulseLengthCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDIOPulseLengthCallback);
+  store->SetUid(HALSIM_RegisterDIOPulseLengthCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double DIOSim::GetPulseLength() const {
+  return HALSIM_GetDIOPulseLength(m_index);
+}
+
+void DIOSim::SetPulseLength(double pulseLength) {
+  HALSIM_SetDIOPulseLength(m_index, pulseLength);
+}
+
+std::unique_ptr<CallbackStore> DIOSim::RegisterIsInputCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDIOIsInputCallback);
+  store->SetUid(HALSIM_RegisterDIOIsInputCallback(m_index, &CallbackStoreThunk,
+                                                  store.get(), initialNotify));
+  return store;
+}
+
+bool DIOSim::GetIsInput() const { return HALSIM_GetDIOIsInput(m_index); }
+
+void DIOSim::SetIsInput(bool isInput) {
+  HALSIM_SetDIOIsInput(m_index, isInput);
+}
+
+std::unique_ptr<CallbackStore> DIOSim::RegisterFilterIndexCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDIOFilterIndexCallback);
+  store->SetUid(HALSIM_RegisterDIOFilterIndexCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int DIOSim::GetFilterIndex() const { return HALSIM_GetDIOFilterIndex(m_index); }
+
+void DIOSim::SetFilterIndex(int filterIndex) {
+  HALSIM_SetDIOFilterIndex(m_index, filterIndex);
+}
+
+void DIOSim::ResetData() { HALSIM_ResetDIOData(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
new file mode 100644
index 0000000..51ed94a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -0,0 +1,134 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/DifferentialDrivetrainSim.h"
+
+#include <frc/system/plant/LinearSystemId.h>
+
+#include "frc/system/RungeKutta.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DifferentialDrivetrainSim::DifferentialDrivetrainSim(
+    const LinearSystem<2, 2, 2>& plant, units::meter_t trackWidth,
+    DCMotor driveMotor, double gearRatio, units::meter_t wheelRadius)
+    : m_plant(plant),
+      m_rb(trackWidth / 2.0),
+      m_wheelRadius(wheelRadius),
+      m_motor(driveMotor),
+      m_originalGearing(gearRatio),
+      m_currentGearing(gearRatio) {
+  m_x.setZero();
+  m_u.setZero();
+}
+
+DifferentialDrivetrainSim::DifferentialDrivetrainSim(
+    frc::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J,
+    units::kilogram_t mass, units::meter_t wheelRadius,
+    units::meter_t trackWidth)
+    : DifferentialDrivetrainSim(
+          frc::LinearSystemId::DrivetrainVelocitySystem(
+              driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
+          trackWidth, driveMotor, gearing, wheelRadius) {}
+
+void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage,
+                                          units::volt_t rightVoltage) {
+  m_u << leftVoltage.to<double>(), rightVoltage.to<double>();
+}
+
+void DifferentialDrivetrainSim::SetGearing(double newGearing) {
+  m_currentGearing = newGearing;
+}
+
+void DifferentialDrivetrainSim::Update(units::second_t dt) {
+  m_x = RungeKutta([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x,
+                   m_u, dt);
+}
+
+double DifferentialDrivetrainSim::GetState(int state) const {
+  return m_x(state);
+}
+
+double DifferentialDrivetrainSim::GetGearing() const {
+  return m_currentGearing;
+}
+
+Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::GetState() const {
+  return m_x;
+}
+
+Rotation2d DifferentialDrivetrainSim::GetHeading() const {
+  return Rotation2d(units::radian_t(m_x(State::kHeading)));
+}
+
+Pose2d DifferentialDrivetrainSim::GetPose() const {
+  return Pose2d(units::meter_t(m_x(State::kX)), units::meter_t(m_x(State::kY)),
+                Rotation2d(units::radian_t(m_x(State::kHeading))));
+}
+
+units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
+  auto loadIleft =
+      m_motor.Current(units::radians_per_second_t(m_x(State::kLeftVelocity) *
+                                                  m_currentGearing /
+                                                  m_wheelRadius.to<double>()),
+                      units::volt_t(m_u(0))) *
+      wpi::sgn(m_u(0));
+
+  auto loadIRight =
+      m_motor.Current(units::radians_per_second_t(m_x(State::kRightVelocity) *
+                                                  m_currentGearing /
+                                                  m_wheelRadius.to<double>()),
+                      units::volt_t(m_u(1))) *
+      wpi::sgn(m_u(1));
+
+  return loadIleft + loadIRight;
+}
+
+void DifferentialDrivetrainSim::SetState(
+    const Eigen::Matrix<double, 7, 1>& state) {
+  m_x = state;
+}
+
+void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) {
+  m_x(State::kX) = pose.X().to<double>();
+  m_x(State::kY) = pose.Y().to<double>();
+  m_x(State::kHeading) = pose.Rotation().Radians().to<double>();
+  m_x(State::kLeftPosition) = 0;
+  m_x(State::kRightPosition) = 0;
+}
+
+Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::Dynamics(
+    const Eigen::Matrix<double, 7, 1>& x,
+    const Eigen::Matrix<double, 2, 1>& u) {
+  // Because G^2 can be factored out of A, we can divide by the old ratio
+  // squared and multiply by the new ratio squared to get a new drivetrain
+  // model.
+  Eigen::Matrix<double, 4, 2> B;
+  B.block<2, 2>(0, 0) = m_plant.B() * m_currentGearing * m_currentGearing /
+                        m_originalGearing / m_originalGearing;
+  B.block<2, 2>(2, 0).setZero();
+
+  // Because G can be factored out of B, we can divide by the old ratio and
+  // multiply by the new ratio to get a new drivetrain model.
+  Eigen::Matrix<double, 4, 4> A;
+  A.block<2, 2>(0, 0) = m_plant.A() * m_currentGearing / m_originalGearing;
+
+  A.block<2, 2>(2, 0).setIdentity();
+  A.block<4, 2>(0, 2).setZero();
+
+  double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
+
+  Eigen::Matrix<double, 7, 1> xdot;
+  xdot(0) = v * std::cos(x(State::kHeading));
+  xdot(1) = v * std::sin(x(State::kHeading));
+  xdot(2) =
+      ((x(State::kRightVelocity) - x(State::kLeftVelocity)) / (2.0 * m_rb))
+          .to<double>();
+  xdot.block<4, 1>(3, 0) = A * x.block<4, 1>(3, 0) + B * u;
+  return xdot;
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp b/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp
new file mode 100644
index 0000000..2ee55ce
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp
@@ -0,0 +1,81 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/DigitalPWMSim.h"
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include <hal/simulation/DigitalPWMData.h>
+
+#include "frc/DigitalOutput.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DigitalPWMSim::DigitalPWMSim(const DigitalOutput& digitalOutput)
+    : m_index{digitalOutput.GetChannel()} {}
+
+DigitalPWMSim DigitalPWMSim::CreateForChannel(int channel) {
+  int index = HALSIM_FindDigitalPWMForChannel(channel);
+  if (index < 0) throw std::out_of_range("no digital PWM found for channel");
+  return DigitalPWMSim{index};
+}
+
+DigitalPWMSim DigitalPWMSim::CreateForIndex(int index) {
+  return DigitalPWMSim{index};
+}
+
+std::unique_ptr<CallbackStore> DigitalPWMSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDigitalPWMInitializedCallback);
+  store->SetUid(HALSIM_RegisterDigitalPWMInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DigitalPWMSim::GetInitialized() const {
+  return HALSIM_GetDigitalPWMInitialized(m_index);
+}
+
+void DigitalPWMSim::SetInitialized(bool initialized) {
+  HALSIM_SetDigitalPWMInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> DigitalPWMSim::RegisterDutyCycleCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDigitalPWMDutyCycleCallback);
+  store->SetUid(HALSIM_RegisterDigitalPWMDutyCycleCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double DigitalPWMSim::GetDutyCycle() const {
+  return HALSIM_GetDigitalPWMDutyCycle(m_index);
+}
+
+void DigitalPWMSim::SetDutyCycle(double dutyCycle) {
+  HALSIM_SetDigitalPWMDutyCycle(m_index, dutyCycle);
+}
+
+std::unique_ptr<CallbackStore> DigitalPWMSim::RegisterPinCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDigitalPWMPinCallback);
+  store->SetUid(HALSIM_RegisterDigitalPWMPinCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int DigitalPWMSim::GetPin() const { return HALSIM_GetDigitalPWMPin(m_index); }
+
+void DigitalPWMSim::SetPin(int pin) { HALSIM_SetDigitalPWMPin(m_index, pin); }
+
+void DigitalPWMSim::ResetData() { HALSIM_ResetDigitalPWMData(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp
new file mode 100644
index 0000000..8c0bf70
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp
@@ -0,0 +1,255 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/DriverStationSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/DriverStationData.h>
+#include <hal/simulation/MockHooks.h>
+
+#include "frc/DriverStation.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterEnabledCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationEnabledCallback);
+  store->SetUid(HALSIM_RegisterDriverStationEnabledCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetEnabled() { return HALSIM_GetDriverStationEnabled(); }
+
+void DriverStationSim::SetEnabled(bool enabled) {
+  HALSIM_SetDriverStationEnabled(enabled);
+}
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterAutonomousCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationAutonomousCallback);
+  store->SetUid(HALSIM_RegisterDriverStationAutonomousCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetAutonomous() {
+  return HALSIM_GetDriverStationAutonomous();
+}
+
+void DriverStationSim::SetAutonomous(bool autonomous) {
+  HALSIM_SetDriverStationAutonomous(autonomous);
+}
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterTestCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationTestCallback);
+  store->SetUid(HALSIM_RegisterDriverStationTestCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetTest() { return HALSIM_GetDriverStationTest(); }
+
+void DriverStationSim::SetTest(bool test) { HALSIM_SetDriverStationTest(test); }
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterEStopCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationEStopCallback);
+  store->SetUid(HALSIM_RegisterDriverStationEStopCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetEStop() { return HALSIM_GetDriverStationEStop(); }
+
+void DriverStationSim::SetEStop(bool eStop) {
+  HALSIM_SetDriverStationEStop(eStop);
+}
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterFmsAttachedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationFmsAttachedCallback);
+  store->SetUid(HALSIM_RegisterDriverStationFmsAttachedCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetFmsAttached() {
+  return HALSIM_GetDriverStationFmsAttached();
+}
+
+void DriverStationSim::SetFmsAttached(bool fmsAttached) {
+  HALSIM_SetDriverStationFmsAttached(fmsAttached);
+}
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterDsAttachedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationDsAttachedCallback);
+  store->SetUid(HALSIM_RegisterDriverStationDsAttachedCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DriverStationSim::GetDsAttached() {
+  return HALSIM_GetDriverStationDsAttached();
+}
+
+void DriverStationSim::SetDsAttached(bool dsAttached) {
+  HALSIM_SetDriverStationDsAttached(dsAttached);
+}
+
+std::unique_ptr<CallbackStore>
+DriverStationSim::RegisterAllianceStationIdCallback(NotifyCallback callback,
+                                                    bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationAllianceStationIdCallback);
+  store->SetUid(HALSIM_RegisterDriverStationAllianceStationIdCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+HAL_AllianceStationID DriverStationSim::GetAllianceStationId() {
+  return HALSIM_GetDriverStationAllianceStationId();
+}
+
+void DriverStationSim::SetAllianceStationId(
+    HAL_AllianceStationID allianceStationId) {
+  HALSIM_SetDriverStationAllianceStationId(allianceStationId);
+}
+
+std::unique_ptr<CallbackStore> DriverStationSim::RegisterMatchTimeCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelDriverStationMatchTimeCallback);
+  store->SetUid(HALSIM_RegisterDriverStationMatchTimeCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double DriverStationSim::GetMatchTime() {
+  return HALSIM_GetDriverStationMatchTime();
+}
+
+void DriverStationSim::SetMatchTime(double matchTime) {
+  HALSIM_SetDriverStationMatchTime(matchTime);
+}
+
+void DriverStationSim::NotifyNewData() {
+  HALSIM_NotifyDriverStationNewData();
+  DriverStation::GetInstance().WaitForData();
+}
+
+void DriverStationSim::SetSendError(bool shouldSend) {
+  if (shouldSend) {
+    HALSIM_SetSendError(nullptr);
+  } else {
+    HALSIM_SetSendError([](HAL_Bool isError, int32_t errorCode,
+                           HAL_Bool isLVCode, const char* details,
+                           const char* location, const char* callStack,
+                           HAL_Bool printMsg) { return 0; });
+  }
+}
+
+void DriverStationSim::SetSendConsoleLine(bool shouldSend) {
+  if (shouldSend) {
+    HALSIM_SetSendConsoleLine(nullptr);
+  } else {
+    HALSIM_SetSendConsoleLine([](const char* line) { return 0; });
+  }
+}
+
+int64_t DriverStationSim::GetJoystickOutputs(int stick) {
+  int64_t outputs = 0;
+  int32_t leftRumble;
+  int32_t rightRumble;
+  HALSIM_GetJoystickOutputs(stick, &outputs, &leftRumble, &rightRumble);
+  return outputs;
+}
+
+int DriverStationSim::GetJoystickRumble(int stick, int rumbleNum) {
+  int64_t outputs;
+  int32_t leftRumble = 0;
+  int32_t rightRumble = 0;
+  HALSIM_GetJoystickOutputs(stick, &outputs, &leftRumble, &rightRumble);
+  return rumbleNum == 0 ? leftRumble : rightRumble;
+}
+
+void DriverStationSim::SetJoystickButton(int stick, int button, bool state) {
+  HALSIM_SetJoystickButton(stick, button, state);
+}
+
+void DriverStationSim::SetJoystickAxis(int stick, int axis, double value) {
+  HALSIM_SetJoystickAxis(stick, axis, value);
+}
+
+void DriverStationSim::SetJoystickPOV(int stick, int pov, int value) {
+  HALSIM_SetJoystickPOV(stick, pov, value);
+}
+
+void DriverStationSim::SetJoystickButtons(int stick, uint32_t buttons) {
+  HALSIM_SetJoystickButtonsValue(stick, buttons);
+}
+
+void DriverStationSim::SetJoystickAxisCount(int stick, int count) {
+  HALSIM_SetJoystickAxisCount(stick, count);
+}
+
+void DriverStationSim::SetJoystickPOVCount(int stick, int count) {
+  HALSIM_SetJoystickPOVCount(stick, count);
+}
+
+void DriverStationSim::SetJoystickButtonCount(int stick, int count) {
+  HALSIM_SetJoystickButtonCount(stick, count);
+}
+
+void DriverStationSim::SetJoystickIsXbox(int stick, bool isXbox) {
+  HALSIM_SetJoystickIsXbox(stick, isXbox);
+}
+
+void DriverStationSim::SetJoystickType(int stick, int type) {
+  HALSIM_SetJoystickType(stick, type);
+}
+
+void DriverStationSim::SetJoystickName(int stick, const char* name) {
+  HALSIM_SetJoystickName(stick, name);
+}
+
+void DriverStationSim::SetJoystickAxisType(int stick, int axis, int type) {
+  HALSIM_SetJoystickAxisType(stick, axis, type);
+}
+
+void DriverStationSim::SetGameSpecificMessage(const char* message) {
+  HALSIM_SetGameSpecificMessage(message);
+}
+
+void DriverStationSim::SetEventName(const char* name) {
+  HALSIM_SetEventName(name);
+}
+
+void DriverStationSim::SetMatchType(DriverStation::MatchType type) {
+  HALSIM_SetMatchType(static_cast<HAL_MatchType>(static_cast<int>(type)));
+}
+
+void DriverStationSim::SetMatchNumber(int matchNumber) {
+  HALSIM_SetMatchNumber(matchNumber);
+}
+
+void DriverStationSim::SetReplayNumber(int replayNumber) {
+  HALSIM_SetReplayNumber(replayNumber);
+}
+
+void DriverStationSim::ResetData() { HALSIM_ResetDriverStationData(); }
diff --git a/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp
new file mode 100644
index 0000000..b12f00a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/DutyCycleEncoderSim.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/DutyCycleEncoder.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+DutyCycleEncoderSim::DutyCycleEncoderSim(const frc::DutyCycleEncoder& encoder) {
+  wpi::SmallString<128> fullname;
+  wpi::raw_svector_ostream os(fullname);
+  os << "DutyCycleEncoder" << '[' << encoder.GetFPGAIndex() << ']';
+  frc::sim::SimDeviceSim deviceSim{fullname.c_str()};
+  m_simPosition = deviceSim.GetDouble("Position");
+  m_simDistancePerRotation = deviceSim.GetDouble("DistancePerRotation");
+}
+
+void DutyCycleEncoderSim::Set(units::turn_t turns) {
+  m_simPosition.Set(turns.to<double>());
+}
+
+void DutyCycleEncoderSim::SetDistance(double distance) {
+  m_simPosition.Set(distance / m_simDistancePerRotation.Get());
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp b/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp
new file mode 100644
index 0000000..7ec19fd
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp
@@ -0,0 +1,85 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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/simulation/DutyCycleSim.h"
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include <hal/simulation/DutyCycleData.h>
+
+#include "frc/DutyCycle.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DutyCycleSim::DutyCycleSim(const DutyCycle& dutyCycle)
+    : m_index{dutyCycle.GetFPGAIndex()} {}
+
+DutyCycleSim DutyCycleSim::CreateForChannel(int channel) {
+  int index = HALSIM_FindDutyCycleForChannel(channel);
+  if (index < 0) throw std::out_of_range("no duty cycle found for channel");
+  return DutyCycleSim{index};
+}
+
+DutyCycleSim DutyCycleSim::CreateForIndex(int index) {
+  return DutyCycleSim{index};
+}
+
+std::unique_ptr<CallbackStore> DutyCycleSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDutyCycleInitializedCallback);
+  store->SetUid(HALSIM_RegisterDutyCycleInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool DutyCycleSim::GetInitialized() const {
+  return HALSIM_GetDutyCycleInitialized(m_index);
+}
+
+void DutyCycleSim::SetInitialized(bool initialized) {
+  HALSIM_SetDutyCycleInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> DutyCycleSim::RegisterFrequencyCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDutyCycleFrequencyCallback);
+  store->SetUid(HALSIM_RegisterDutyCycleFrequencyCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int DutyCycleSim::GetFrequency() const {
+  return HALSIM_GetDutyCycleFrequency(m_index);
+}
+
+void DutyCycleSim::SetFrequency(int count) {
+  HALSIM_SetDutyCycleFrequency(m_index, count);
+}
+
+std::unique_ptr<CallbackStore> DutyCycleSim::RegisterOutputCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelDutyCycleOutputCallback);
+  store->SetUid(HALSIM_RegisterDutyCycleOutputCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double DutyCycleSim::GetOutput() const {
+  return HALSIM_GetDutyCycleOutput(m_index);
+}
+
+void DutyCycleSim::SetOutput(double period) {
+  HALSIM_SetDutyCycleOutput(m_index, period);
+}
+
+void DutyCycleSim::ResetData() { HALSIM_ResetDutyCycleData(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
new file mode 100644
index 0000000..89c09cb
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/ElevatorSim.h"
+
+#include <wpi/MathExtras.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/RungeKutta.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
+                         const DCMotor& gearbox, double gearing,
+                         units::meter_t drumRadius, units::meter_t minHeight,
+                         units::meter_t maxHeight,
+                         const std::array<double, 1>& measurementStdDevs)
+    : LinearSystemSim(plant, measurementStdDevs),
+      m_gearbox(gearbox),
+      m_drumRadius(drumRadius),
+      m_minHeight(minHeight),
+      m_maxHeight(maxHeight),
+      m_gearing(gearing) {}
+
+ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
+                         units::kilogram_t carriageMass,
+                         units::meter_t drumRadius, units::meter_t minHeight,
+                         units::meter_t maxHeight,
+                         const std::array<double, 1>& measurementStdDevs)
+    : LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
+                                                     drumRadius, gearing),
+                      measurementStdDevs),
+      m_gearbox(gearbox),
+      m_drumRadius(drumRadius),
+      m_minHeight(minHeight),
+      m_maxHeight(maxHeight),
+      m_gearing(gearing) {}
+
+bool ElevatorSim::HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) < m_minHeight.to<double>();
+}
+
+bool ElevatorSim::HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) > m_maxHeight.to<double>();
+}
+
+units::meter_t ElevatorSim::GetPosition() const {
+  return units::meter_t{m_x(0)};
+}
+
+units::meters_per_second_t ElevatorSim::GetVelocity() const {
+  return units::meters_per_second_t{m_x(1)};
+}
+
+units::ampere_t ElevatorSim::GetCurrentDraw() const {
+  // I = V / R - omega / (Kv * R)
+  // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+  // is spinning 10x faster than the output.
+
+  // v = r w, so w = v / r
+  units::meters_per_second_t velocity{m_x(1)};
+  units::radians_per_second_t motorVelocity =
+      velocity / m_drumRadius * m_gearing * 1_rad;
+
+  // Perform calculation and return.
+  return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
+}
+
+void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
+  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+}
+
+Eigen::Matrix<double, 2, 1> ElevatorSim::UpdateX(
+    const Eigen::Matrix<double, 2, 1>& currentXhat,
+    const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
+  auto updatedXhat = RungeKutta(
+      [&](const Eigen::Matrix<double, 2, 1>& x,
+          const Eigen::Matrix<double, 1, 1>& u_)
+          -> Eigen::Matrix<double, 2, 1> {
+        return m_plant.A() * x + m_plant.B() * u_ + MakeMatrix<2, 1>(0.0, -9.8);
+      },
+      currentXhat, u, dt);
+  // Check for collision after updating x-hat.
+  if (HasHitLowerLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_minHeight.to<double>(), 0.0);
+  }
+  if (HasHitUpperLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_maxHeight.to<double>(), 0.0);
+  }
+  return updatedXhat;
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp
new file mode 100644
index 0000000..5bc5d52
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp
@@ -0,0 +1,189 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/EncoderSim.h"
+
+#include <memory>
+#include <stdexcept>
+#include <utility>
+
+#include <hal/simulation/EncoderData.h>
+
+#include "frc/Encoder.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+EncoderSim::EncoderSim(const Encoder& encoder)
+    : m_index{encoder.GetFPGAIndex()} {}
+
+EncoderSim EncoderSim::CreateForChannel(int channel) {
+  int index = HALSIM_FindEncoderForChannel(channel);
+  if (index < 0) throw std::out_of_range("no encoder found for channel");
+  return EncoderSim{index};
+}
+
+EncoderSim EncoderSim::CreateForIndex(int index) { return EncoderSim{index}; }
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderInitializedCallback);
+  store->SetUid(HALSIM_RegisterEncoderInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool EncoderSim::GetInitialized() const {
+  return HALSIM_GetEncoderInitialized(m_index);
+}
+
+void EncoderSim::SetInitialized(bool initialized) {
+  HALSIM_SetEncoderInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterCountCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderCountCallback);
+  store->SetUid(HALSIM_RegisterEncoderCountCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int EncoderSim::GetCount() const { return HALSIM_GetEncoderCount(m_index); }
+
+void EncoderSim::SetCount(int count) { HALSIM_SetEncoderCount(m_index, count); }
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterPeriodCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderPeriodCallback);
+  store->SetUid(HALSIM_RegisterEncoderPeriodCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double EncoderSim::GetPeriod() const {
+  return HALSIM_GetEncoderPeriod(m_index);
+}
+
+void EncoderSim::SetPeriod(double period) {
+  HALSIM_SetEncoderPeriod(m_index, period);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterResetCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderResetCallback);
+  store->SetUid(HALSIM_RegisterEncoderResetCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool EncoderSim::GetReset() const { return HALSIM_GetEncoderReset(m_index); }
+
+void EncoderSim::SetReset(bool reset) {
+  HALSIM_SetEncoderReset(m_index, reset);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterMaxPeriodCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderMaxPeriodCallback);
+  store->SetUid(HALSIM_RegisterEncoderMaxPeriodCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double EncoderSim::GetMaxPeriod() const {
+  return HALSIM_GetEncoderMaxPeriod(m_index);
+}
+
+void EncoderSim::SetMaxPeriod(double maxPeriod) {
+  HALSIM_SetEncoderMaxPeriod(m_index, maxPeriod);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterDirectionCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderDirectionCallback);
+  store->SetUid(HALSIM_RegisterEncoderDirectionCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool EncoderSim::GetDirection() const {
+  return HALSIM_GetEncoderDirection(m_index);
+}
+
+void EncoderSim::SetDirection(bool direction) {
+  HALSIM_SetEncoderDirection(m_index, direction);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterReverseDirectionCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderReverseDirectionCallback);
+  store->SetUid(HALSIM_RegisterEncoderReverseDirectionCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool EncoderSim::GetReverseDirection() const {
+  return HALSIM_GetEncoderReverseDirection(m_index);
+}
+
+void EncoderSim::SetReverseDirection(bool reverseDirection) {
+  HALSIM_SetEncoderReverseDirection(m_index, reverseDirection);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterSamplesToAverageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderSamplesToAverageCallback);
+  store->SetUid(HALSIM_RegisterEncoderSamplesToAverageCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int EncoderSim::GetSamplesToAverage() const {
+  return HALSIM_GetEncoderSamplesToAverage(m_index);
+}
+
+void EncoderSim::SetSamplesToAverage(int samplesToAverage) {
+  HALSIM_SetEncoderSamplesToAverage(m_index, samplesToAverage);
+}
+
+std::unique_ptr<CallbackStore> EncoderSim::RegisterDistancePerPulseCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelEncoderDistancePerPulseCallback);
+  store->SetUid(HALSIM_RegisterEncoderDistancePerPulseCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double EncoderSim::GetDistancePerPulse() const {
+  return HALSIM_GetEncoderDistancePerPulse(m_index);
+}
+
+void EncoderSim::SetDistancePerPulse(double distancePerPulse) {
+  HALSIM_SetEncoderDistancePerPulse(m_index, distancePerPulse);
+}
+
+void EncoderSim::ResetData() { HALSIM_ResetEncoderData(m_index); }
+
+void EncoderSim::SetDistance(double distance) {
+  HALSIM_SetEncoderDistance(m_index, distance);
+}
+
+double EncoderSim::GetDistance() { return HALSIM_GetEncoderDistance(m_index); }
+
+void EncoderSim::SetRate(double rate) { HALSIM_SetEncoderRate(m_index, rate); }
+
+double EncoderSim::GetRate() { return HALSIM_GetEncoderRate(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/Field2d.cpp b/wpilibc/src/main/native/cpp/simulation/Field2d.cpp
new file mode 100644
index 0000000..a6098b6
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/Field2d.cpp
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/Field2d.h"
+
+using namespace frc;
+
+Field2d::Field2d() : m_device{"Field2D"} {
+  if (m_device) {
+    m_x = m_device.CreateDouble("x", false, 0.0);
+    m_y = m_device.CreateDouble("y", false, 0.0);
+    m_rot = m_device.CreateDouble("rot", false, 0.0);
+  }
+}
+
+void Field2d::SetRobotPose(const Pose2d& pose) {
+  if (m_device) {
+    auto& translation = pose.Translation();
+    m_x.Set(translation.X().to<double>());
+    m_y.Set(translation.Y().to<double>());
+    m_rot.Set(pose.Rotation().Degrees().to<double>());
+  } else {
+    m_pose = pose;
+  }
+}
+
+void Field2d::SetRobotPose(units::meter_t x, units::meter_t y,
+                           Rotation2d rotation) {
+  if (m_device) {
+    m_x.Set(x.to<double>());
+    m_y.Set(y.to<double>());
+    m_rot.Set(rotation.Degrees().to<double>());
+  } else {
+    m_pose = Pose2d{x, y, rotation};
+  }
+}
+
+Pose2d Field2d::GetRobotPose() {
+  if (m_device) {
+    return Pose2d{units::meter_t{m_x.Get()}, units::meter_t{m_y.Get()},
+                  Rotation2d{units::degree_t{m_rot.Get()}}};
+  } else {
+    return m_pose;
+  }
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
new file mode 100644
index 0000000..65fc3c8
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/FlywheelSim.h"
+
+#include <wpi/MathExtras.h>
+
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+FlywheelSim::FlywheelSim(const LinearSystem<1, 1, 1>& plant,
+                         const DCMotor& gearbox, double gearing,
+                         const std::array<double, 1>& measurementStdDevs)
+    : LinearSystemSim<1, 1, 1>(plant, measurementStdDevs),
+      m_gearbox(gearbox),
+      m_gearing(gearing) {}
+
+FlywheelSim::FlywheelSim(const DCMotor& gearbox, double gearing,
+                         units::kilogram_square_meter_t moi,
+                         const std::array<double, 1>& measurementStdDevs)
+    : FlywheelSim(LinearSystemId::FlywheelSystem(gearbox, moi, gearing),
+                  gearbox, gearing, measurementStdDevs) {}
+
+units::radians_per_second_t FlywheelSim::GetAngularVelocity() const {
+  return units::radians_per_second_t{GetOutput(0)};
+}
+
+units::ampere_t FlywheelSim::GetCurrentDraw() const {
+  // I = V / R - omega / (Kv * R)
+  // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+  // is spinning 10x faster than the output.
+  return m_gearbox.Current(GetAngularVelocity() * m_gearing,
+                           units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
+}
+
+void FlywheelSim::SetInputVoltage(units::volt_t voltage) {
+  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/GenericHIDSim.cpp b/wpilibc/src/main/native/cpp/simulation/GenericHIDSim.cpp
new file mode 100644
index 0000000..8df265a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/GenericHIDSim.cpp
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/GenericHIDSim.h"
+
+#include "frc/GenericHID.h"
+#include "frc/simulation/DriverStationSim.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+GenericHIDSim::GenericHIDSim(const GenericHID& joystick)
+    : m_port{joystick.GetPort()} {}
+
+GenericHIDSim::GenericHIDSim(int port) : m_port{port} {}
+
+void GenericHIDSim::NotifyNewData() { DriverStationSim::NotifyNewData(); }
+
+void GenericHIDSim::SetRawButton(int button, bool value) {
+  DriverStationSim::SetJoystickButton(m_port, button, value);
+}
+
+void GenericHIDSim::SetRawAxis(int axis, double value) {
+  DriverStationSim::SetJoystickAxis(m_port, axis, value);
+}
+
+void GenericHIDSim::SetPOV(int pov, int value) {
+  DriverStationSim::SetJoystickPOV(m_port, pov, value);
+}
+
+void GenericHIDSim::SetPOV(int value) { SetPOV(0, value); }
+
+void GenericHIDSim::SetAxisCount(int count) {
+  DriverStationSim::SetJoystickAxisCount(m_port, count);
+}
+
+void GenericHIDSim::SetPOVCount(int count) {
+  DriverStationSim::SetJoystickPOVCount(m_port, count);
+}
+
+void GenericHIDSim::SetButtonCount(int count) {
+  DriverStationSim::SetJoystickButtonCount(m_port, count);
+}
+
+void GenericHIDSim::SetType(GenericHID::HIDType type) {
+  DriverStationSim::SetJoystickType(m_port, type);
+}
+
+void GenericHIDSim::SetName(const char* name) {
+  DriverStationSim::SetJoystickName(m_port, name);
+}
+
+void GenericHIDSim::SetAxisType(int axis, int type) {
+  DriverStationSim::SetJoystickAxisType(m_port, axis, type);
+}
+
+bool GenericHIDSim::GetOutput(int outputNumber) {
+  int64_t outputs = GetOutputs();
+  return (outputs & (static_cast<int64_t>(1) << (outputNumber - 1))) != 0;
+}
+
+int64_t GenericHIDSim::GetOutputs() {
+  return DriverStationSim::GetJoystickOutputs(m_port);
+}
+
+double GenericHIDSim::GetRumble(GenericHID::RumbleType type) {
+  int value = DriverStationSim::GetJoystickRumble(
+      m_port, type == GenericHID::kLeftRumble ? 0 : 1);
+  return value / 65535.0;
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/JoystickSim.cpp b/wpilibc/src/main/native/cpp/simulation/JoystickSim.cpp
new file mode 100644
index 0000000..55f6089
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/JoystickSim.cpp
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/JoystickSim.h"
+
+#include "frc/Joystick.h"
+#include "frc/simulation/GenericHIDSim.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+JoystickSim::JoystickSim(const Joystick& joystick)
+    : GenericHIDSim{joystick}, m_joystick{&joystick} {
+  // default to a reasonable joystick configuration
+  SetAxisCount(5);
+  SetButtonCount(12);
+  SetPOVCount(1);
+}
+
+JoystickSim::JoystickSim(int port) : GenericHIDSim{port} {
+  // default to a reasonable joystick configuration
+  SetAxisCount(5);
+  SetButtonCount(12);
+  SetPOVCount(1);
+}
+
+void JoystickSim::SetX(double value) {
+  SetRawAxis(
+      m_joystick ? m_joystick->GetXChannel() : Joystick::kDefaultXChannel,
+      value);
+}
+
+void JoystickSim::SetY(double value) {
+  SetRawAxis(
+      m_joystick ? m_joystick->GetYChannel() : Joystick::kDefaultYChannel,
+      value);
+}
+
+void JoystickSim::SetZ(double value) {
+  SetRawAxis(
+      m_joystick ? m_joystick->GetZChannel() : Joystick::kDefaultZChannel,
+      value);
+}
+
+void JoystickSim::SetTwist(double value) {
+  SetRawAxis(m_joystick ? m_joystick->GetTwistChannel()
+                        : Joystick::kDefaultTwistChannel,
+             value);
+}
+
+void JoystickSim::SetThrottle(double value) {
+  SetRawAxis(m_joystick ? m_joystick->GetThrottleChannel()
+                        : Joystick::kDefaultThrottleChannel,
+             value);
+}
+
+void JoystickSim::SetTrigger(bool state) { SetRawButton(1, state); }
+
+void JoystickSim::SetTop(bool state) { SetRawButton(2, state); }
diff --git a/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp b/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp
new file mode 100644
index 0000000..43178b1
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp
@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/Mechanism2D.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/Twine.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+Mechanism2D::Mechanism2D() : m_device{"Mechanism2D"} {}
+
+void Mechanism2D::SetLigamentAngle(const wpi::Twine& ligamentPath,
+                                   float angle) {
+  if (m_device) {
+    wpi::SmallString<64> fullPathBuf;
+    wpi::StringRef fullPath =
+        (ligamentPath + "/angle").toNullTerminatedStringRef(fullPathBuf);
+    if (!createdItems.count(fullPath)) {
+      createdItems[fullPath] =
+          m_device.CreateDouble(fullPath.data(), false, angle);
+    }
+    createdItems[fullPath].Set(angle);
+  }
+}
+
+void Mechanism2D::SetLigamentLength(const wpi::Twine& ligamentPath,
+                                    float length) {
+  if (m_device) {
+    wpi::SmallString<64> fullPathBuf;
+    wpi::StringRef fullPath =
+        (ligamentPath + "/length").toNullTerminatedStringRef(fullPathBuf);
+    if (!createdItems.count(fullPath)) {
+      createdItems[fullPath] =
+          m_device.CreateDouble(fullPath.data(), false, length);
+    }
+    createdItems[fullPath].Set(length);
+  }
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/PCMSim.cpp b/wpilibc/src/main/native/cpp/simulation/PCMSim.cpp
new file mode 100644
index 0000000..5d0566a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/PCMSim.cpp
@@ -0,0 +1,158 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/PCMSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/PCMData.h>
+
+#include "frc/Compressor.h"
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+PCMSim::PCMSim() : m_index{SensorUtil::GetDefaultSolenoidModule()} {}
+
+PCMSim::PCMSim(int module) : m_index{module} {}
+
+PCMSim::PCMSim(const Compressor& compressor)
+    : m_index{compressor.GetModule()} {}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterSolenoidInitializedCallback(
+    int channel, NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, channel, -1, callback,
+      &HALSIM_CancelPCMSolenoidInitializedCallback);
+  store->SetUid(HALSIM_RegisterPCMSolenoidInitializedCallback(
+      m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetSolenoidInitialized(int channel) const {
+  return HALSIM_GetPCMSolenoidInitialized(m_index, channel);
+}
+
+void PCMSim::SetSolenoidInitialized(int channel, bool solenoidInitialized) {
+  HALSIM_SetPCMSolenoidInitialized(m_index, channel, solenoidInitialized);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterSolenoidOutputCallback(
+    int channel, NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, channel, -1, callback, &HALSIM_CancelPCMSolenoidOutputCallback);
+  store->SetUid(HALSIM_RegisterPCMSolenoidOutputCallback(
+      m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetSolenoidOutput(int channel) const {
+  return HALSIM_GetPCMSolenoidOutput(m_index, channel);
+}
+
+void PCMSim::SetSolenoidOutput(int channel, bool solenoidOutput) {
+  HALSIM_SetPCMSolenoidOutput(m_index, channel, solenoidOutput);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterCompressorInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPCMCompressorInitializedCallback);
+  store->SetUid(HALSIM_RegisterPCMCompressorInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetCompressorInitialized() const {
+  return HALSIM_GetPCMCompressorInitialized(m_index);
+}
+
+void PCMSim::SetCompressorInitialized(bool compressorInitialized) {
+  HALSIM_SetPCMCompressorInitialized(m_index, compressorInitialized);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterCompressorOnCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPCMCompressorOnCallback);
+  store->SetUid(HALSIM_RegisterPCMCompressorOnCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetCompressorOn() const {
+  return HALSIM_GetPCMCompressorOn(m_index);
+}
+
+void PCMSim::SetCompressorOn(bool compressorOn) {
+  HALSIM_SetPCMCompressorOn(m_index, compressorOn);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterClosedLoopEnabledCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPCMClosedLoopEnabledCallback);
+  store->SetUid(HALSIM_RegisterPCMClosedLoopEnabledCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetClosedLoopEnabled() const {
+  return HALSIM_GetPCMClosedLoopEnabled(m_index);
+}
+
+void PCMSim::SetClosedLoopEnabled(bool closedLoopEnabled) {
+  HALSIM_SetPCMClosedLoopEnabled(m_index, closedLoopEnabled);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterPressureSwitchCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPCMPressureSwitchCallback);
+  store->SetUid(HALSIM_RegisterPCMPressureSwitchCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PCMSim::GetPressureSwitch() const {
+  return HALSIM_GetPCMPressureSwitch(m_index);
+}
+
+void PCMSim::SetPressureSwitch(bool pressureSwitch) {
+  HALSIM_SetPCMPressureSwitch(m_index, pressureSwitch);
+}
+
+std::unique_ptr<CallbackStore> PCMSim::RegisterCompressorCurrentCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPCMCompressorCurrentCallback);
+  store->SetUid(HALSIM_RegisterPCMCompressorCurrentCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double PCMSim::GetCompressorCurrent() const {
+  return HALSIM_GetPCMCompressorCurrent(m_index);
+}
+
+void PCMSim::SetCompressorCurrent(double compressorCurrent) {
+  HALSIM_SetPCMCompressorCurrent(m_index, compressorCurrent);
+}
+
+uint8_t PCMSim::GetAllSolenoidOutputs() const {
+  uint8_t ret = 0;
+  HALSIM_GetPCMAllSolenoids(m_index, &ret);
+  return ret;
+}
+
+void PCMSim::SetAllSolenoidOutputs(uint8_t outputs) {
+  HALSIM_SetPCMAllSolenoids(m_index, outputs);
+}
+
+void PCMSim::ResetData() { HALSIM_ResetPCMData(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/PDPSim.cpp b/wpilibc/src/main/native/cpp/simulation/PDPSim.cpp
new file mode 100644
index 0000000..26d6a45
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/PDPSim.cpp
@@ -0,0 +1,98 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/PDPSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/PDPData.h>
+
+#include "frc/PowerDistributionPanel.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+PDPSim::PDPSim(int module) : m_index{module} {}
+
+PDPSim::PDPSim(const PowerDistributionPanel& pdp) : m_index{pdp.GetModule()} {}
+
+std::unique_ptr<CallbackStore> PDPSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPDPInitializedCallback);
+  store->SetUid(HALSIM_RegisterPDPInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PDPSim::GetInitialized() const {
+  return HALSIM_GetPDPInitialized(m_index);
+}
+
+void PDPSim::SetInitialized(bool initialized) {
+  HALSIM_SetPDPInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> PDPSim::RegisterTemperatureCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPDPTemperatureCallback);
+  store->SetUid(HALSIM_RegisterPDPTemperatureCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double PDPSim::GetTemperature() const {
+  return HALSIM_GetPDPTemperature(m_index);
+}
+
+void PDPSim::SetTemperature(double temperature) {
+  HALSIM_SetPDPTemperature(m_index, temperature);
+}
+
+std::unique_ptr<CallbackStore> PDPSim::RegisterVoltageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPDPVoltageCallback);
+  store->SetUid(HALSIM_RegisterPDPVoltageCallback(m_index, &CallbackStoreThunk,
+                                                  store.get(), initialNotify));
+  return store;
+}
+
+double PDPSim::GetVoltage() const { return HALSIM_GetPDPVoltage(m_index); }
+
+void PDPSim::SetVoltage(double voltage) {
+  HALSIM_SetPDPVoltage(m_index, voltage);
+}
+
+std::unique_ptr<CallbackStore> PDPSim::RegisterCurrentCallback(
+    int channel, NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, channel, -1, callback, &HALSIM_CancelPDPCurrentCallback);
+  store->SetUid(HALSIM_RegisterPDPCurrentCallback(
+      m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double PDPSim::GetCurrent(int channel) const {
+  return HALSIM_GetPDPCurrent(m_index, channel);
+}
+
+void PDPSim::SetCurrent(int channel, double current) {
+  HALSIM_SetPDPCurrent(m_index, channel, current);
+}
+
+void PDPSim::GetAllCurrents(double* currents) const {
+  HALSIM_GetPDPAllCurrents(m_index, currents);
+}
+
+void PDPSim::SetAllCurrents(const double* currents) {
+  HALSIM_SetPDPAllCurrents(m_index, currents);
+}
+
+void PDPSim::ResetData() { HALSIM_ResetPDPData(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp b/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp
new file mode 100644
index 0000000..2fae8fb
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp
@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/PWMSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/PWMData.h>
+
+#include "frc/PWM.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+PWMSim::PWMSim(const PWM& pwm) : m_index{pwm.GetChannel()} {}
+
+PWMSim::PWMSim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPWMInitializedCallback);
+  store->SetUid(HALSIM_RegisterPWMInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PWMSim::GetInitialized() const {
+  return HALSIM_GetPWMInitialized(m_index);
+}
+
+void PWMSim::SetInitialized(bool initialized) {
+  HALSIM_SetPWMInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterRawValueCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPWMRawValueCallback);
+  store->SetUid(HALSIM_RegisterPWMRawValueCallback(m_index, &CallbackStoreThunk,
+                                                   store.get(), initialNotify));
+  return store;
+}
+
+int PWMSim::GetRawValue() const { return HALSIM_GetPWMRawValue(m_index); }
+
+void PWMSim::SetRawValue(int rawValue) {
+  HALSIM_SetPWMRawValue(m_index, rawValue);
+}
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterSpeedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(m_index, -1, callback,
+                                               &HALSIM_CancelPWMSpeedCallback);
+  store->SetUid(HALSIM_RegisterPWMSpeedCallback(m_index, &CallbackStoreThunk,
+                                                store.get(), initialNotify));
+  return store;
+}
+
+double PWMSim::GetSpeed() const { return HALSIM_GetPWMSpeed(m_index); }
+
+void PWMSim::SetSpeed(double speed) { HALSIM_SetPWMSpeed(m_index, speed); }
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterPositionCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPWMPositionCallback);
+  store->SetUid(HALSIM_RegisterPWMPositionCallback(m_index, &CallbackStoreThunk,
+                                                   store.get(), initialNotify));
+  return store;
+}
+
+double PWMSim::GetPosition() const { return HALSIM_GetPWMPosition(m_index); }
+
+void PWMSim::SetPosition(double position) {
+  HALSIM_SetPWMPosition(m_index, position);
+}
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterPeriodScaleCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPWMPeriodScaleCallback);
+  store->SetUid(HALSIM_RegisterPWMPeriodScaleCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int PWMSim::GetPeriodScale() const { return HALSIM_GetPWMPeriodScale(m_index); }
+
+void PWMSim::SetPeriodScale(int periodScale) {
+  HALSIM_SetPWMPeriodScale(m_index, periodScale);
+}
+
+std::unique_ptr<CallbackStore> PWMSim::RegisterZeroLatchCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPWMZeroLatchCallback);
+  store->SetUid(HALSIM_RegisterPWMZeroLatchCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PWMSim::GetZeroLatch() const { return HALSIM_GetPWMZeroLatch(m_index); }
+
+void PWMSim::SetZeroLatch(bool zeroLatch) {
+  HALSIM_SetPWMZeroLatch(m_index, zeroLatch);
+}
+
+void PWMSim::ResetData() { HALSIM_ResetPWMData(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp b/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp
new file mode 100644
index 0000000..7f7aa65
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/RelaySim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/RelayData.h>
+
+#include "frc/Relay.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+RelaySim::RelaySim(const Relay& relay) : m_index{relay.GetChannel()} {}
+
+RelaySim::RelaySim(int channel) : m_index{channel} {}
+
+std::unique_ptr<CallbackStore> RelaySim::RegisterInitializedForwardCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelRelayInitializedForwardCallback);
+  store->SetUid(HALSIM_RegisterRelayInitializedForwardCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RelaySim::GetInitializedForward() const {
+  return HALSIM_GetRelayInitializedForward(m_index);
+}
+
+void RelaySim::SetInitializedForward(bool initializedForward) {
+  HALSIM_SetRelayInitializedForward(m_index, initializedForward);
+}
+
+std::unique_ptr<CallbackStore> RelaySim::RegisterInitializedReverseCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelRelayInitializedReverseCallback);
+  store->SetUid(HALSIM_RegisterRelayInitializedReverseCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RelaySim::GetInitializedReverse() const {
+  return HALSIM_GetRelayInitializedReverse(m_index);
+}
+
+void RelaySim::SetInitializedReverse(bool initializedReverse) {
+  HALSIM_SetRelayInitializedReverse(m_index, initializedReverse);
+}
+
+std::unique_ptr<CallbackStore> RelaySim::RegisterForwardCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelRelayForwardCallback);
+  store->SetUid(HALSIM_RegisterRelayForwardCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RelaySim::GetForward() const { return HALSIM_GetRelayForward(m_index); }
+
+void RelaySim::SetForward(bool forward) {
+  HALSIM_SetRelayForward(m_index, forward);
+}
+
+std::unique_ptr<CallbackStore> RelaySim::RegisterReverseCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelRelayReverseCallback);
+  store->SetUid(HALSIM_RegisterRelayReverseCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RelaySim::GetReverse() const { return HALSIM_GetRelayReverse(m_index); }
+
+void RelaySim::SetReverse(bool reverse) {
+  HALSIM_SetRelayReverse(m_index, reverse);
+}
+
+void RelaySim::ResetData() { HALSIM_ResetRelayData(m_index); }
diff --git a/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp b/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
new file mode 100644
index 0000000..0d1d104
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
@@ -0,0 +1,259 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/RoboRioSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/RoboRioData.h>
+
+using namespace frc;
+using namespace frc::sim;
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterFPGAButtonCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioFPGAButtonCallback);
+  store->SetUid(HALSIM_RegisterRoboRioFPGAButtonCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RoboRioSim::GetFPGAButton() { return HALSIM_GetRoboRioFPGAButton(); }
+
+void RoboRioSim::SetFPGAButton(bool fPGAButton) {
+  HALSIM_SetRoboRioFPGAButton(fPGAButton);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterVInVoltageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioVInVoltageCallback);
+  store->SetUid(HALSIM_RegisterRoboRioVInVoltageCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::volt_t RoboRioSim::GetVInVoltage() {
+  return units::volt_t(HALSIM_GetRoboRioVInVoltage());
+}
+
+void RoboRioSim::SetVInVoltage(units::volt_t vInVoltage) {
+  HALSIM_SetRoboRioVInVoltage(vInVoltage.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterVInCurrentCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioVInCurrentCallback);
+  store->SetUid(HALSIM_RegisterRoboRioVInCurrentCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::ampere_t RoboRioSim::GetVInCurrent() {
+  return units::ampere_t(HALSIM_GetRoboRioVInCurrent());
+}
+
+void RoboRioSim::SetVInCurrent(units::ampere_t vInCurrent) {
+  HALSIM_SetRoboRioVInCurrent(vInCurrent.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserVoltage6VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserVoltage6VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserVoltage6VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::volt_t RoboRioSim::GetUserVoltage6V() {
+  return units::volt_t(HALSIM_GetRoboRioUserVoltage6V());
+}
+
+void RoboRioSim::SetUserVoltage6V(units::volt_t userVoltage6V) {
+  HALSIM_SetRoboRioUserVoltage6V(userVoltage6V.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent6VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserCurrent6VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserCurrent6VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::ampere_t RoboRioSim::GetUserCurrent6V() {
+  return units::ampere_t(HALSIM_GetRoboRioUserCurrent6V());
+}
+
+void RoboRioSim::SetUserCurrent6V(units::ampere_t userCurrent6V) {
+  HALSIM_SetRoboRioUserCurrent6V(userCurrent6V.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive6VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserActive6VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserActive6VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RoboRioSim::GetUserActive6V() { return HALSIM_GetRoboRioUserActive6V(); }
+
+void RoboRioSim::SetUserActive6V(bool userActive6V) {
+  HALSIM_SetRoboRioUserActive6V(userActive6V);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserVoltage5VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserVoltage5VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserVoltage5VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::volt_t RoboRioSim::GetUserVoltage5V() {
+  return units::volt_t(HALSIM_GetRoboRioUserVoltage5V());
+}
+
+void RoboRioSim::SetUserVoltage5V(units::volt_t userVoltage5V) {
+  HALSIM_SetRoboRioUserVoltage5V(userVoltage5V.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent5VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserCurrent5VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserCurrent5VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::ampere_t RoboRioSim::GetUserCurrent5V() {
+  return units::ampere_t(HALSIM_GetRoboRioUserCurrent5V());
+}
+
+void RoboRioSim::SetUserCurrent5V(units::ampere_t userCurrent5V) {
+  HALSIM_SetRoboRioUserCurrent5V(userCurrent5V.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive5VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserActive5VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserActive5VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RoboRioSim::GetUserActive5V() { return HALSIM_GetRoboRioUserActive5V(); }
+
+void RoboRioSim::SetUserActive5V(bool userActive5V) {
+  HALSIM_SetRoboRioUserActive5V(userActive5V);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserVoltage3V3Callback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserVoltage3V3Callback);
+  store->SetUid(HALSIM_RegisterRoboRioUserVoltage3V3Callback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::volt_t RoboRioSim::GetUserVoltage3V3() {
+  return units::volt_t(HALSIM_GetRoboRioUserVoltage3V3());
+}
+
+void RoboRioSim::SetUserVoltage3V3(units::volt_t userVoltage3V3) {
+  HALSIM_SetRoboRioUserVoltage3V3(userVoltage3V3.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent3V3Callback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserCurrent3V3Callback);
+  store->SetUid(HALSIM_RegisterRoboRioUserCurrent3V3Callback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::ampere_t RoboRioSim::GetUserCurrent3V3() {
+  return units::ampere_t(HALSIM_GetRoboRioUserCurrent3V3());
+}
+
+void RoboRioSim::SetUserCurrent3V3(units::ampere_t userCurrent3V3) {
+  HALSIM_SetRoboRioUserCurrent3V3(userCurrent3V3.to<double>());
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive3V3Callback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserActive3V3Callback);
+  store->SetUid(HALSIM_RegisterRoboRioUserActive3V3Callback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool RoboRioSim::GetUserActive3V3() { return HALSIM_GetRoboRioUserActive3V3(); }
+
+void RoboRioSim::SetUserActive3V3(bool userActive3V3) {
+  HALSIM_SetRoboRioUserActive3V3(userActive3V3);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserFaults6VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserFaults6VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserFaults6VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int RoboRioSim::GetUserFaults6V() { return HALSIM_GetRoboRioUserFaults6V(); }
+
+void RoboRioSim::SetUserFaults6V(int userFaults6V) {
+  HALSIM_SetRoboRioUserFaults6V(userFaults6V);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserFaults5VCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserFaults5VCallback);
+  store->SetUid(HALSIM_RegisterRoboRioUserFaults5VCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int RoboRioSim::GetUserFaults5V() { return HALSIM_GetRoboRioUserFaults5V(); }
+
+void RoboRioSim::SetUserFaults5V(int userFaults5V) {
+  HALSIM_SetRoboRioUserFaults5V(userFaults5V);
+}
+
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserFaults3V3Callback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioUserFaults3V3Callback);
+  store->SetUid(HALSIM_RegisterRoboRioUserFaults3V3Callback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int RoboRioSim::GetUserFaults3V3() { return HALSIM_GetRoboRioUserFaults3V3(); }
+
+void RoboRioSim::SetUserFaults3V3(int userFaults3V3) {
+  HALSIM_SetRoboRioUserFaults3V3(userFaults3V3);
+}
+
+void ResetData() { HALSIM_ResetRoboRioData(); }
diff --git a/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp b/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp
new file mode 100644
index 0000000..b71f99d
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp
@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/SPIAccelerometerSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/SPIAccelerometerData.h>
+
+using namespace frc;
+using namespace frc::sim;
+
+SPIAccelerometerSim::SPIAccelerometerSim(int index) { m_index = index; }
+
+std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterActiveCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelSPIAccelerometerActiveCallback);
+  store->SetUid(HALSIM_RegisterSPIAccelerometerActiveCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool SPIAccelerometerSim::GetActive() const {
+  return HALSIM_GetSPIAccelerometerActive(m_index);
+}
+
+void SPIAccelerometerSim::SetActive(bool active) {
+  HALSIM_SetSPIAccelerometerActive(m_index, active);
+}
+
+std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterRangeCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelSPIAccelerometerRangeCallback);
+  store->SetUid(HALSIM_RegisterSPIAccelerometerRangeCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+int SPIAccelerometerSim::GetRange() const {
+  return HALSIM_GetSPIAccelerometerRange(m_index);
+}
+
+void SPIAccelerometerSim::SetRange(int range) {
+  HALSIM_SetSPIAccelerometerRange(m_index, range);
+}
+
+std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterXCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelSPIAccelerometerXCallback);
+  store->SetUid(HALSIM_RegisterSPIAccelerometerXCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double SPIAccelerometerSim::GetX() const {
+  return HALSIM_GetSPIAccelerometerX(m_index);
+}
+
+void SPIAccelerometerSim::SetX(double x) {
+  HALSIM_SetSPIAccelerometerX(m_index, x);
+}
+
+std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterYCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelSPIAccelerometerYCallback);
+  store->SetUid(HALSIM_RegisterSPIAccelerometerYCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double SPIAccelerometerSim::GetY() const {
+  return HALSIM_GetSPIAccelerometerY(m_index);
+}
+
+void SPIAccelerometerSim::SetY(double y) {
+  HALSIM_SetSPIAccelerometerY(m_index, y);
+}
+
+std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterZCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelSPIAccelerometerZCallback);
+  store->SetUid(HALSIM_RegisterSPIAccelerometerZCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double SPIAccelerometerSim::GetZ() const {
+  return HALSIM_GetSPIAccelerometerZ(m_index);
+}
+
+void SPIAccelerometerSim::SetZ(double z) {
+  HALSIM_SetSPIAccelerometerZ(m_index, z);
+}
+
+void SPIAccelerometerSim::ResetData() {
+  HALSIM_ResetSPIAccelerometerData(m_index);
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp b/wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp
new file mode 100644
index 0000000..3d52cdf
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp
@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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/simulation/SimDeviceSim.h"
+
+#include <string>
+#include <vector>
+
+#include <hal/SimDevice.h>
+#include <hal/simulation/SimDeviceData.h>
+
+using namespace frc;
+using namespace frc::sim;
+
+SimDeviceSim::SimDeviceSim(const char* name)
+    : m_handle{HALSIM_GetSimDeviceHandle(name)} {}
+
+hal::SimValue SimDeviceSim::GetValue(const char* name) const {
+  return HALSIM_GetSimValueHandle(m_handle, name);
+}
+
+hal::SimDouble SimDeviceSim::GetDouble(const char* name) const {
+  return HALSIM_GetSimValueHandle(m_handle, name);
+}
+
+hal::SimEnum SimDeviceSim::GetEnum(const char* name) const {
+  return HALSIM_GetSimValueHandle(m_handle, name);
+}
+
+hal::SimBoolean SimDeviceSim::GetBoolean(const char* name) const {
+  return HALSIM_GetSimValueHandle(m_handle, name);
+}
+
+std::vector<std::string> SimDeviceSim::GetEnumOptions(hal::SimEnum val) {
+  int32_t numOptions;
+  const char** options = HALSIM_GetSimValueEnumOptions(val, &numOptions);
+  std::vector<std::string> rv;
+  rv.reserve(numOptions);
+  for (int32_t i = 0; i < numOptions; ++i) rv.emplace_back(options[i]);
+  return rv;
+}
+
+void SimDeviceSim::ResetData() { HALSIM_ResetSimDeviceData(); }
diff --git a/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp b/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp
new file mode 100644
index 0000000..28c434c
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/SimHooks.h"
+
+#include <hal/simulation/MockHooks.h>
+
+namespace frc {
+namespace sim {
+
+void SetRuntimeType(HAL_RuntimeType type) { HALSIM_SetRuntimeType(type); }
+
+void WaitForProgramStart() { HALSIM_WaitForProgramStart(); }
+
+void SetProgramStarted() { HALSIM_SetProgramStarted(); }
+
+bool GetProgramStarted() { return HALSIM_GetProgramStarted(); }
+
+void RestartTiming() { HALSIM_RestartTiming(); }
+
+void PauseTiming() { HALSIM_PauseTiming(); }
+
+void ResumeTiming() { HALSIM_ResumeTiming(); }
+
+bool IsTimingPaused() { return HALSIM_IsTimingPaused(); }
+
+void StepTiming(units::second_t delta) {
+  HALSIM_StepTiming(static_cast<uint64_t>(delta.to<double>() * 1e6));
+}
+
+void StepTimingAsync(units::second_t delta) {
+  HALSIM_StepTimingAsync(static_cast<uint64_t>(delta.to<double>() * 1e6));
+}
+
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
new file mode 100644
index 0000000..b33a19e
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/SingleJointedArmSim.h"
+
+#include <cmath>
+
+#include <units/voltage.h>
+#include <wpi/MathExtras.h>
+
+#include "frc/system/RungeKutta.h"
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+SingleJointedArmSim::SingleJointedArmSim(
+    const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing,
+    units::meter_t armLength, units::radian_t minAngle,
+    units::radian_t maxAngle, units::kilogram_t mass, bool simulateGravity,
+    const std::array<double, 1>& measurementStdDevs)
+    : LinearSystemSim<2, 1, 1>(system, measurementStdDevs),
+      m_r(armLength),
+      m_minAngle(minAngle),
+      m_maxAngle(maxAngle),
+      m_mass(mass),
+      m_gearbox(gearbox),
+      m_gearing(gearing),
+      m_simulateGravity(simulateGravity) {}
+
+SingleJointedArmSim::SingleJointedArmSim(
+    const DCMotor& gearbox, double gearing, units::kilogram_square_meter_t moi,
+    units::meter_t armLength, units::radian_t minAngle,
+    units::radian_t maxAngle, units::kilogram_t mass, bool simulateGravity,
+    const std::array<double, 1>& measurementStdDevs)
+    : SingleJointedArmSim(
+          LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing),
+          gearbox, gearing, armLength, minAngle, maxAngle, mass,
+          simulateGravity, measurementStdDevs) {}
+
+bool SingleJointedArmSim::HasHitLowerLimit(
+    const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) < m_minAngle.to<double>();
+}
+
+bool SingleJointedArmSim::HasHitUpperLimit(
+    const Eigen::Matrix<double, 2, 1>& x) const {
+  return x(0) > m_maxAngle.to<double>();
+}
+
+units::radian_t SingleJointedArmSim::GetAngle() const {
+  return units::radian_t{m_x(0)};
+}
+
+units::radians_per_second_t SingleJointedArmSim::GetVelocity() const {
+  return units::radians_per_second_t{m_x(1)};
+}
+
+units::ampere_t SingleJointedArmSim::GetCurrentDraw() const {
+  // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+  // is spinning 10x faster than the output
+  units::radians_per_second_t motorVelocity{m_x(1) * m_gearing};
+  return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) *
+         wpi::sgn(m_u(0));
+}
+
+void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) {
+  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+}
+
+Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
+    const Eigen::Matrix<double, 2, 1>& currentXhat,
+    const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
+  // Horizontal case:
+  // Torque = F * r = I * alpha
+  // alpha = F * r / I
+  // Since F = mg,
+  // alpha = m * g * r / I
+  // Finally, multiply RHS by cos(theta) to account for the arm angle
+  // This acceleration is added to the linear system dynamics x-dot = Ax + Bu
+  // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I *
+  // std::cos(theta)]]
+
+  auto updatedXhat = RungeKutta(
+      [&](const auto& x, const auto& u) -> Eigen::Matrix<double, 2, 1> {
+        Eigen::Matrix<double, 2, 1> xdot = m_plant.A() * x + m_plant.B() * u;
+
+        if (m_simulateGravity) {
+          xdot += MakeMatrix<2, 1>(0.0, (m_mass * m_r * -9.8 * 3.0 /
+                                         (m_mass * m_r * m_r) * std::cos(x(0)))
+                                            .template to<double>());
+        }
+        return xdot;
+      },
+      currentXhat, u, dt);
+
+  // Check for collisions.
+  if (HasHitLowerLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_minAngle.to<double>(), 0.0);
+  } else if (HasHitUpperLimit(updatedXhat)) {
+    return MakeMatrix<2, 1>(m_maxAngle.to<double>(), 0.0);
+  }
+  return updatedXhat;
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/XboxControllerSim.cpp b/wpilibc/src/main/native/cpp/simulation/XboxControllerSim.cpp
new file mode 100644
index 0000000..e3bbdce
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/XboxControllerSim.cpp
@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/XboxControllerSim.h"
+
+#include "frc/XboxController.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+XboxControllerSim::XboxControllerSim(const XboxController& joystick)
+    : GenericHIDSim{joystick} {
+  SetAxisCount(6);
+  SetButtonCount(10);
+}
+
+XboxControllerSim::XboxControllerSim(int port) : GenericHIDSim{port} {
+  SetAxisCount(6);
+  SetButtonCount(10);
+}
+
+void XboxControllerSim::SetX(GenericHID::JoystickHand hand, double value) {
+  if (hand == GenericHID::kLeftHand) {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kLeftX), value);
+  } else {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kRightX), value);
+  }
+}
+
+void XboxControllerSim::SetY(GenericHID::JoystickHand hand, double value) {
+  if (hand == GenericHID::kLeftHand) {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kLeftY), value);
+  } else {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kRightY), value);
+  }
+}
+
+void XboxControllerSim::SetTriggerAxis(GenericHID::JoystickHand hand,
+                                       double value) {
+  if (hand == GenericHID::kLeftHand) {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kLeftTrigger), value);
+  } else {
+    SetRawAxis(static_cast<int>(XboxController::Axis::kRightTrigger), value);
+  }
+}
+
+void XboxControllerSim::SetBumper(GenericHID::JoystickHand hand, bool state) {
+  if (hand == GenericHID::kLeftHand) {
+    SetRawButton(static_cast<int>(XboxController::Button::kBumperLeft), state);
+  } else {
+    SetRawButton(static_cast<int>(XboxController::Button::kBumperRight), state);
+  }
+}
+
+void XboxControllerSim::SetStickButton(GenericHID::JoystickHand hand,
+                                       bool state) {
+  if (hand == GenericHID::kLeftHand) {
+    SetRawButton(static_cast<int>(XboxController::Button::kStickLeft), state);
+  } else {
+    SetRawButton(static_cast<int>(XboxController::Button::kStickRight), state);
+  }
+}
+
+void XboxControllerSim::SetAButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kA), state);
+}
+
+void XboxControllerSim::SetBButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kB), state);
+}
+
+void XboxControllerSim::SetXButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kX), state);
+}
+
+void XboxControllerSim::SetYButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kY), state);
+}
+
+void XboxControllerSim::SetBackButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kBack), state);
+}
+
+void XboxControllerSim::SetStartButton(bool state) {
+  SetRawButton(static_cast<int>(XboxController::Button::kStart), state);
+}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
index d075deb..135f4be 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 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.                                                               */
@@ -32,7 +32,9 @@
   for (auto& property : m_properties) {
     if (property.update) property.update(property.entry, time);
   }
-  if (m_updateTable) m_updateTable();
+  for (auto& updateTable : m_updateTables) {
+    updateTable();
+  }
 }
 
 void SendableBuilderImpl::StartListeners() {
@@ -71,7 +73,7 @@
 }
 
 void SendableBuilderImpl::SetUpdateTable(std::function<void()> func) {
-  m_updateTable = func;
+  m_updateTables.emplace_back(std::move(func));
 }
 
 nt::NetworkTableEntry SendableBuilderImpl::GetEntry(const wpi::Twine& key) {
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
index 9d57ad6..4480313 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -163,7 +163,9 @@
 void SendableRegistry::Move(Sendable* to, Sendable* from) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(from);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   UID compUid = it->getSecond();
   m_impl->componentMap.erase(it);
   m_impl->componentMap[to] = compUid;
@@ -188,14 +190,18 @@
 std::string SendableRegistry::GetName(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return std::string{};
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return std::string{};
   return m_impl->components[it->getSecond() - 1]->name;
 }
 
 void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->name = name.str();
 }
 
@@ -203,7 +209,9 @@
                                int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->SetName(moduleType, channel);
 }
 
@@ -211,7 +219,9 @@
                                int moduleNumber, int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->SetName(moduleType, moduleNumber,
                                                    channel);
 }
@@ -220,7 +230,9 @@
                                const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   auto& comp = *m_impl->components[it->getSecond() - 1];
   comp.name = name.str();
   comp.subsystem = subsystem.str();
@@ -229,7 +241,9 @@
 std::string SendableRegistry::GetSubsystem(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return std::string{};
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return std::string{};
   return m_impl->components[it->getSecond() - 1]->subsystem;
 }
 
@@ -237,7 +251,9 @@
                                     const wpi::Twine& subsystem) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->subsystem = subsystem.str();
 }
 
@@ -251,7 +267,9 @@
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return nullptr;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return nullptr;
   auto& comp = *m_impl->components[it->getSecond() - 1];
   std::shared_ptr<void> rv;
   if (static_cast<size_t>(handle) < comp.data.size())
@@ -267,7 +285,9 @@
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return nullptr;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return nullptr;
   auto& comp = *m_impl->components[it->getSecond() - 1];
   if (static_cast<size_t>(handle) >= comp.data.size()) return nullptr;
   return comp.data[handle];
@@ -276,14 +296,18 @@
 void SendableRegistry::EnableLiveWindow(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->liveWindow = true;
 }
 
 void SendableRegistry::DisableLiveWindow(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->liveWindow = false;
 }
 
@@ -298,13 +322,17 @@
 Sendable* SendableRegistry::GetSendable(UID uid) {
   if (uid == 0) return nullptr;
   std::scoped_lock lock(m_impl->mutex);
+  if ((uid - 1) >= m_impl->components.size() || !m_impl->components[uid - 1])
+    return nullptr;
   return m_impl->components[uid - 1]->sendable;
 }
 
 void SendableRegistry::Publish(UID sendableUid,
                                std::shared_ptr<NetworkTable> table) {
   std::scoped_lock lock(m_impl->mutex);
-  if (sendableUid == 0) return;
+  if (sendableUid == 0 || (sendableUid - 1) >= m_impl->components.size() ||
+      !m_impl->components[sendableUid - 1])
+    return;
   auto& comp = *m_impl->components[sendableUid - 1];
   comp.builder = SendableBuilderImpl{};  // clear any current builder
   comp.builder.SetTable(table);
@@ -316,6 +344,9 @@
 void SendableRegistry::Update(UID sendableUid) {
   if (sendableUid == 0) return;
   std::scoped_lock lock(m_impl->mutex);
+  if ((sendableUid - 1) >= m_impl->components.size() ||
+      !m_impl->components[sendableUid - 1])
+    return;
   m_impl->components[sendableUid - 1]->builder.UpdateTable();
 }
 
@@ -327,7 +358,7 @@
   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 (comp && comp->sendable && comp->liveWindow) {
       if (static_cast<size_t>(dataHandle) >= comp->data.size())
         comp->data.resize(dataHandle + 1);
       CallbackData cbdata{comp->sendable,         comp->name,
diff --git a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
deleted file mode 100644
index 3991fec..0000000
--- a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/spline/CubicHermiteSpline.h"
-
-using namespace frc;
-
-CubicHermiteSpline::CubicHermiteSpline(
-    std::array<double, 2> xInitialControlVector,
-    std::array<double, 2> xFinalControlVector,
-    std::array<double, 2> yInitialControlVector,
-    std::array<double, 2> yFinalControlVector) {
-  const auto hermite = MakeHermiteBasis();
-  const auto x =
-      ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
-  const auto y =
-      ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
-
-  // Populate first two rows with coefficients.
-  m_coefficients.template block<1, 4>(0, 0) = hermite * x;
-  m_coefficients.template block<1, 4>(1, 0) = hermite * y;
-
-  // 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) * (2 - i);
-  }
-}
diff --git a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
deleted file mode 100644
index bb8ad3c..0000000
--- a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
+++ /dev/null
@@ -1,45 +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/spline/QuinticHermiteSpline.h"
-
-using namespace frc;
-
-QuinticHermiteSpline::QuinticHermiteSpline(
-    std::array<double, 3> xInitialControlVector,
-    std::array<double, 3> xFinalControlVector,
-    std::array<double, 3> yInitialControlVector,
-    std::array<double, 3> yFinalControlVector) {
-  const auto hermite = MakeHermiteBasis();
-  const auto x =
-      ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
-  const auto y =
-      ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
-
-  // Populate first two rows with coefficients.
-  m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose();
-  m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose();
-
-  // 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) * (4 - i);
-  }
-}
diff --git a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp b/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
deleted file mode 100644
index cbfc8c1..0000000
--- a/wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
+++ /dev/null
@@ -1,208 +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/spline/SplineHelper.h"
-
-#include <cstddef>
-
-using namespace frc;
-
-std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
-    const Spline<3>::ControlVector& start, std::vector<Translation2d> waypoints,
-    const Spline<3>::ControlVector& end) {
-  std::vector<CubicHermiteSpline> splines;
-
-  std::array<double, 2> xInitial = start.x;
-  std::array<double, 2> yInitial = start.y;
-  std::array<double, 2> xFinal = end.x;
-  std::array<double, 2> yFinal = end.y;
-
-  if (waypoints.size() > 1) {
-    waypoints.emplace(waypoints.begin(),
-                      Translation2d{units::meter_t(xInitial[0]),
-                                    units::meter_t(yInitial[0])});
-    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);
-      c.emplace_back(1);
-    }
-    c.emplace_back(0);
-
-    // populate rhs vectors
-    dx.emplace_back(
-        3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
-        xInitial[1]);
-    dy.emplace_back(
-        3 * (waypoints[2].Y().to<double>() - waypoints[0].Y().to<double>()) -
-        yInitial[1]);
-    if (waypoints.size() > 4) {
-      for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
-        dx.emplace_back(3 * (waypoints[i + 1].X().to<double>() -
-                             waypoints[i - 1].X().to<double>()));
-        dy.emplace_back(3 * (waypoints[i + 1].Y().to<double>() -
-                             waypoints[i - 1].Y().to<double>()));
-      }
-    }
-    dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
-                         waypoints[waypoints.size() - 3].X().to<double>()) -
-                    xFinal[1]);
-    dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
-                         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);
-
-    fx.emplace(fx.begin(), xInitial[1]);
-    fx.emplace_back(xFinal[1]);
-    fy.emplace(fy.begin(), yInitial[1]);
-    fy.emplace_back(yFinal[1]);
-
-    for (size_t i = 0; i < fx.size() - 1; ++i) {
-      // Create the spline.
-      const CubicHermiteSpline spline{
-          {waypoints[i].X().to<double>(), fx[i]},
-          {waypoints[i + 1].X().to<double>(), fx[i + 1]},
-          {waypoints[i].Y().to<double>(), fy[i]},
-          {waypoints[i + 1].Y().to<double>(), fy[i + 1]}};
-
-      splines.push_back(spline);
-    }
-  } else if (waypoints.size() == 1) {
-    const double xDeriv =
-        (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
-    const double yDeriv =
-        (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
-
-    std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
-                                            xDeriv};
-    std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
-                                            yDeriv};
-
-    splines.emplace_back(xInitial, midXControlVector, yInitial,
-                         midYControlVector);
-    splines.emplace_back(midXControlVector, xFinal, midYControlVector, yFinal);
-
-  } else {
-    // Create the spline.
-    const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal};
-    splines.push_back(spline);
-  }
-
-  return splines;
-}
-
-std::vector<QuinticHermiteSpline>
-SplineHelper::QuinticSplinesFromControlVectors(
-    const std::vector<Spline<5>::ControlVector>& controlVectors) {
-  std::vector<QuinticHermiteSpline> splines;
-  for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
-    auto& xInitial = controlVectors[i].x;
-    auto& yInitial = controlVectors[i].y;
-    auto& xFinal = controlVectors[i + 1].x;
-    auto& yFinal = controlVectors[i + 1].y;
-    splines.emplace_back(xInitial, xFinal, yInitial, yFinal);
-  }
-  return splines;
-}
-
-std::array<Spline<3>::ControlVector, 2>
-SplineHelper::CubicControlVectorsFromWaypoints(
-    const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
-    const Pose2d& end) {
-  double scalar;
-  if (interiorWaypoints.empty()) {
-    scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
-  } else {
-    scalar =
-        1.2 *
-        start.Translation().Distance(interiorWaypoints.front()).to<double>();
-  }
-  const auto initialCV = CubicControlVector(scalar, start);
-  if (!interiorWaypoints.empty()) {
-    scalar =
-        1.2 * end.Translation().Distance(interiorWaypoints.back()).to<double>();
-  }
-  const auto finalCV = CubicControlVector(scalar, end);
-  return {initialCV, finalCV};
-}
-
-std::vector<Spline<5>::ControlVector>
-SplineHelper::QuinticControlVectorsFromWaypoints(
-    const std::vector<Pose2d>& waypoints) {
-  std::vector<Spline<5>::ControlVector> vectors;
-  for (size_t i = 0; i < waypoints.size() - 1; ++i) {
-    auto& p0 = waypoints[i];
-    auto& p1 = waypoints[i + 1];
-
-    // This just makes the splines look better.
-    const auto scalar =
-        1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
-
-    vectors.push_back(QuinticControlVector(scalar, p0));
-    vectors.push_back(QuinticControlVector(scalar, p1));
-  }
-  return vectors;
-}
-
-void SplineHelper::ThomasAlgorithm(const std::vector<double>& a,
-                                   const std::vector<double>& b,
-                                   const std::vector<double>& c,
-                                   const std::vector<double>& d,
-                                   std::vector<double>* solutionVector) {
-  auto& f = *solutionVector;
-  size_t N = d.size();
-
-  // Create the temporary vectors
-  // Note that this is inefficient as it is possible to call
-  // this function many times. A better implementation would
-  // pass these temporary matrices by non-const reference to
-  // save excess allocation and deallocation
-  std::vector<double> c_star(N, 0.0);
-  std::vector<double> d_star(N, 0.0);
-
-  // This updates the coefficients in the first row
-  // Note that we should be checking for division by zero here
-  c_star[0] = c[0] / b[0];
-  d_star[0] = d[0] / b[0];
-
-  // Create the c_star and d_star coefficients in the forward sweep
-  for (size_t i = 1; i < N; ++i) {
-    double m = 1.0 / (b[i] - a[i] * c_star[i - 1]);
-    c_star[i] = c[i] * m;
-    d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m;
-  }
-
-  f[N - 1] = d_star[N - 1];
-  // This is the reverse sweep, used to update the solution vector f
-  for (int i = N - 2; i >= 0; i--) {
-    f[i] = d_star[i] - c_star[i] * f[i + 1];
-  }
-}
diff --git a/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp b/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp
deleted file mode 100644
index f3c42c6..0000000
--- a/wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp
+++ /dev/null
@@ -1,12 +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/spline/SplineParameterizer.h"
-
-constexpr units::meter_t frc::SplineParameterizer::kMaxDx;
-constexpr units::meter_t frc::SplineParameterizer::kMaxDy;
-constexpr units::radian_t frc::SplineParameterizer::kMaxDtheta;
diff --git a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp b/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
deleted file mode 100644
index 84d4f37..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#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.
-  const auto newT = Lerp(t, endValue.t, i);
-
-  // Find the delta time between the current state and the interpolated state.
-  const auto deltaT = newT - t;
-
-  // If delta time is negative, flip the order of interpolation.
-  if (deltaT < 0_s) return endValue.Interpolate(*this, 1.0 - i);
-
-  // Check whether the robot is reversing at this stage.
-  const auto reversing =
-      velocity < 0_mps ||
-      (units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq);
-
-  // Calculate the new velocity.
-  // v = v_0 + at
-  const units::meters_per_second_t newV = velocity + (acceleration * deltaT);
-
-  // Calculate the change in position.
-  // delta_s = v_0 t + 0.5 at^2
-  const units::meter_t newS =
-      (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
-      (reversing ? -1.0 : 1.0);
-
-  // Return the new state. To find the new position for the new state, we need
-  // to interpolate between the two endpoint poses. The fraction for
-  // interpolation is the change in position (delta s) divided by the total
-  // distance between the two endpoints.
-  const double interpolationFrac =
-      newS / endValue.pose.Translation().Distance(pose.Translation());
-
-  return {newT, newV, acceleration,
-          Lerp(pose, endValue.pose, interpolationFrac),
-          Lerp(curvature, endValue.curvature, interpolationFrac)};
-}
-
-Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
-  m_totalTime = states.back().t;
-}
-
-Trajectory::State Trajectory::Sample(units::second_t t) const {
-  if (t <= m_states.front().t) return m_states.front();
-  if (t >= m_totalTime) return m_states.back();
-
-  // To get the element that we want, we will use a binary search algorithm
-  // instead of iterating over a for-loop. A binary search is O(std::log(n))
-  // whereas searching using a loop is O(n).
-
-  // This starts at 1 because we use the previous state later on for
-  // interpolation.
-  int low = 1;
-  int high = m_states.size() - 1;
-
-  while (low != high) {
-    int mid = (low + high) / 2;
-    if (m_states[mid].t < t) {
-      // This index and everything under it are less than the requested
-      // timestamp. Therefore, we can discard them.
-      low = mid + 1;
-    } else {
-      // t is at least as large as the element at this index. This means that
-      // anything after it cannot be what we are looking for.
-      high = mid;
-    }
-  }
-
-  // High and Low should be the same.
-
-  // The sample's timestamp is now greater than or equal to the requested
-  // timestamp. If it is greater, we need to interpolate between the
-  // previous state and the current state to get the exact state that we
-  // want.
-  const auto sample = m_states[low];
-  const auto prevSample = m_states[low - 1];
-
-  // If the difference in states is negligible, then we are spot on!
-  if (units::math::abs(sample.t - prevSample.t) < 1E-9_s) {
-    return sample;
-  }
-  // Interpolate between the two states for the state that we want.
-  return prevSample.Interpolate(sample,
-                                (t - prevSample.t) / (sample.t - prevSample.t));
-}
-
-Trajectory Trajectory::TransformBy(const Transform2d& transform) {
-  auto& firstState = m_states[0];
-  auto& firstPose = firstState.pose;
-
-  // Calculate the transformed first pose.
-  auto newFirstPose = firstPose + transform;
-  auto newStates = m_states;
-  newStates[0].pose = newFirstPose;
-
-  for (unsigned int i = 1; i < newStates.size(); i++) {
-    auto& state = newStates[i];
-    // We are transforming relative to the coordinate frame of the new initial
-    // pose.
-    state.pose = newFirstPose + (state.pose - firstPose);
-  }
-
-  return Trajectory(newStates);
-}
-
-Trajectory Trajectory::RelativeTo(const Pose2d& pose) {
-  auto newStates = m_states;
-  for (auto& state : newStates) {
-    state.pose = state.pose.RelativeTo(pose);
-  }
-  return Trajectory(newStates);
-}
-
-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/TrajectoryGenerator.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
deleted file mode 100644
index 3c95472..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 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/TrajectoryGenerator.h"
-
-#include <utility>
-
-#include "frc/DriverStation.h"
-#include "frc/spline/SplineHelper.h"
-#include "frc/spline/SplineParameterizer.h"
-#include "frc/trajectory/TrajectoryParameterizer.h"
-
-using namespace frc;
-
-const Trajectory TrajectoryGenerator::kDoNothingTrajectory(
-    std::vector<Trajectory::State>{Trajectory::State()});
-
-Trajectory TrajectoryGenerator::GenerateTrajectory(
-    Spline<3>::ControlVector initial,
-    const std::vector<Translation2d>& interiorWaypoints,
-    Spline<3>::ControlVector end, const TrajectoryConfig& config) {
-  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
-
-  // Make theta normal for trajectory generation if path is reversed.
-  // Flip the headings.
-  if (config.IsReversed()) {
-    initial.x[1] *= -1;
-    initial.y[1] *= -1;
-    end.x[1] *= -1;
-    end.y[1] *= -1;
-  }
-
-  std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
-  try {
-    points =
-        SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
-            initial, interiorWaypoints, end));
-  } catch (SplineParameterizer::MalformedSplineException& e) {
-    DriverStation::ReportError(e.what());
-    return kDoNothingTrajectory;
-  }
-
-  // After trajectory generation, flip theta back so it's relative to the
-  // field. Also fix curvature.
-  if (config.IsReversed()) {
-    for (auto& point : points) {
-      point = {point.first + flip, -point.second};
-    }
-  }
-
-  return TrajectoryParameterizer::TimeParameterizeTrajectory(
-      points, config.Constraints(), config.StartVelocity(),
-      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
-      config.IsReversed());
-}
-
-Trajectory TrajectoryGenerator::GenerateTrajectory(
-    const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
-    const Pose2d& end, const TrajectoryConfig& config) {
-  auto [startCV, endCV] = SplineHelper::CubicControlVectorsFromWaypoints(
-      start, interiorWaypoints, end);
-  return GenerateTrajectory(startCV, interiorWaypoints, endCV, config);
-}
-
-Trajectory TrajectoryGenerator::GenerateTrajectory(
-    std::vector<Spline<5>::ControlVector> controlVectors,
-    const TrajectoryConfig& config) {
-  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
-  // Make theta normal for trajectory generation if path is reversed.
-  if (config.IsReversed()) {
-    for (auto& vector : controlVectors) {
-      // Flip the headings.
-      vector.x[1] *= -1;
-      vector.y[1] *= -1;
-    }
-  }
-
-  std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
-  try {
-    points = SplinePointsFromSplines(
-        SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
-  } catch (SplineParameterizer::MalformedSplineException& e) {
-    DriverStation::ReportError(e.what());
-    return kDoNothingTrajectory;
-  }
-
-  // After trajectory generation, flip theta back so it's relative to the
-  // field. Also fix curvature.
-  if (config.IsReversed()) {
-    for (auto& point : points) {
-      point = {point.first + flip, -point.second};
-    }
-  }
-
-  return TrajectoryParameterizer::TimeParameterizeTrajectory(
-      points, config.Constraints(), config.StartVelocity(),
-      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
-      config.IsReversed());
-}
-
-Trajectory TrajectoryGenerator::GenerateTrajectory(
-    const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
-  return GenerateTrajectory(
-      SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config);
-}
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
deleted file mode 100644
index 131ae8a..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+++ /dev/null
@@ -1,224 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-/*
- * MIT License
- *
- * Copyright (c) 2018 Team 254
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#include "frc/trajectory/TrajectoryParameterizer.h"
-
-using namespace frc;
-
-Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
-    const std::vector<PoseWithCurvature>& points,
-    const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
-    units::meters_per_second_t startVelocity,
-    units::meters_per_second_t endVelocity,
-    units::meters_per_second_t maxVelocity,
-    units::meters_per_second_squared_t maxAcceleration, bool reversed) {
-  std::vector<ConstrainedState> constrainedStates(points.size());
-
-  ConstrainedState predecessor{points.front(), 0_m, startVelocity,
-                               -maxAcceleration, maxAcceleration};
-
-  constrainedStates[0] = predecessor;
-
-  // Forward pass
-  for (unsigned int i = 0; i < points.size(); i++) {
-    auto& constrainedState = constrainedStates[i];
-    constrainedState.pose = points[i];
-
-    // Begin constraining based on predecessor
-    units::meter_t ds = constrainedState.pose.first.Translation().Distance(
-        predecessor.pose.first.Translation());
-    constrainedState.distance = ds + predecessor.distance;
-
-    // We may need to iterate to find the maximum end velocity and common
-    // acceleration, since acceleration limits may be a function of velocity.
-    while (true) {
-      // Enforce global max velocity and max reachable velocity by global
-      // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
-
-      constrainedState.maxVelocity = units::math::min(
-          maxVelocity,
-          units::math::sqrt(predecessor.maxVelocity * predecessor.maxVelocity +
-                            predecessor.maxAcceleration * ds * 2.0));
-
-      constrainedState.minAcceleration = -maxAcceleration;
-      constrainedState.maxAcceleration = maxAcceleration;
-
-      // At this point, the constrained state is fully constructed apart from
-      // all the custom-defined user constraints.
-      for (const auto& constraint : constraints) {
-        constrainedState.maxVelocity = units::math::min(
-            constrainedState.maxVelocity,
-            constraint->MaxVelocity(constrainedState.pose.first,
-                                    constrainedState.pose.second,
-                                    constrainedState.maxVelocity));
-      }
-
-      // Now enforce all acceleration limits.
-      EnforceAccelerationLimits(reversed, constraints, &constrainedState);
-
-      if (ds.to<double>() < kEpsilon) break;
-
-      // If the actual acceleration for this state is higher than the max
-      // acceleration that we applied, then we need to reduce the max
-      // acceleration of the predecessor and try again.
-      units::meters_per_second_squared_t actualAcceleration =
-          (constrainedState.maxVelocity * constrainedState.maxVelocity -
-           predecessor.maxVelocity * predecessor.maxVelocity) /
-          (ds * 2.0);
-
-      // If we violate the max acceleration constraint, let's modify the
-      // predecessor.
-      if (constrainedState.maxAcceleration < actualAcceleration - 1E-6_mps_sq) {
-        predecessor.maxAcceleration = constrainedState.maxAcceleration;
-      } else {
-        // Constrain the predecessor's max acceleration to the current
-        // acceleration.
-        if (actualAcceleration > predecessor.minAcceleration + 1E-6_mps_sq) {
-          predecessor.maxAcceleration = actualAcceleration;
-        }
-        // If the actual acceleration is less than the predecessor's min
-        // acceleration, it will be repaired in the backward pass.
-        break;
-      }
-    }
-    predecessor = constrainedState;
-  }
-
-  ConstrainedState successor{points.back(), constrainedStates.back().distance,
-                             endVelocity, -maxAcceleration, maxAcceleration};
-
-  // Backward pass
-  for (int i = points.size() - 1; i >= 0; i--) {
-    auto& constrainedState = constrainedStates[i];
-    units::meter_t ds =
-        constrainedState.distance - successor.distance;  // negative
-
-    while (true) {
-      // Enforce max velocity limit (reverse)
-      // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
-      units::meters_per_second_t newMaxVelocity =
-          units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
-                            successor.minAcceleration * ds * 2.0);
-
-      // No more limits to impose! This state can be finalized.
-      if (newMaxVelocity >= constrainedState.maxVelocity) break;
-
-      constrainedState.maxVelocity = newMaxVelocity;
-
-      // Check all acceleration constraints with the new max velocity.
-      EnforceAccelerationLimits(reversed, constraints, &constrainedState);
-
-      if (ds.to<double>() > -kEpsilon) break;
-
-      // If the actual acceleration for this state is lower than the min
-      // acceleration, then we need to lower the min acceleration of the
-      // successor and try again.
-      units::meters_per_second_squared_t actualAcceleration =
-          (constrainedState.maxVelocity * constrainedState.maxVelocity -
-           successor.maxVelocity * successor.maxVelocity) /
-          (ds * 2.0);
-      if (constrainedState.minAcceleration > actualAcceleration + 1E-6_mps_sq) {
-        successor.minAcceleration = constrainedState.minAcceleration;
-      } else {
-        successor.minAcceleration = actualAcceleration;
-        break;
-      }
-    }
-    successor = constrainedState;
-  }
-
-  // Now we can integrate the constrained states forward in time to obtain our
-  // trajectory states.
-
-  std::vector<Trajectory::State> states(points.size());
-  units::second_t t = 0_s;
-  units::meter_t s = 0_m;
-  units::meters_per_second_t v = 0_mps;
-
-  for (unsigned int i = 0; i < constrainedStates.size(); i++) {
-    auto state = constrainedStates[i];
-
-    // Calculate the change in position between the current state and the
-    // previous state.
-    units::meter_t ds = state.distance - s;
-
-    // Calculate the acceleration between the current state and the previous
-    // state.
-    units::meters_per_second_squared_t accel =
-        (state.maxVelocity * state.maxVelocity - v * v) / (ds * 2);
-
-    // Calculate dt.
-    units::second_t dt = 0_s;
-    if (i > 0) {
-      states.at(i - 1).acceleration = reversed ? -accel : accel;
-      if (units::math::abs(accel) > 1E-6_mps_sq) {
-        // v_f = v_0 + a * t
-        dt = (state.maxVelocity - v) / accel;
-      } else if (units::math::abs(v) > 1E-6_mps) {
-        // delta_x = v * t
-        dt = ds / v;
-      } else {
-        throw std::runtime_error(
-            "Something went wrong during trajectory generation.");
-      }
-    }
-
-    v = state.maxVelocity;
-    s = state.distance;
-
-    t += dt;
-
-    states[i] = {t, reversed ? -v : v, reversed ? -accel : accel,
-                 state.pose.first, state.pose.second};
-  }
-
-  return Trajectory(states);
-}
-
-void TrajectoryParameterizer::EnforceAccelerationLimits(
-    bool reverse,
-    const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
-    ConstrainedState* state) {
-  for (auto&& constraint : constraints) {
-    double factor = reverse ? -1.0 : 1.0;
-
-    auto minMaxAccel = constraint->MinMaxAcceleration(
-        state->pose.first, state->pose.second, state->maxVelocity * factor);
-
-    state->minAcceleration = units::math::max(
-        state->minAcceleration,
-        reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration);
-
-    state->maxAcceleration = units::math::min(
-        state->maxAcceleration,
-        reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration);
-  }
-}
diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
deleted file mode 100644
index f3cf30d..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
+++ /dev/null
@@ -1,58 +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/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/CentripetalAccelerationConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
deleted file mode 100644
index bf45c34..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
+++ /dev/null
@@ -1,40 +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/constraint/CentripetalAccelerationConstraint.h"
-
-using namespace frc;
-
-CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
-    units::meters_per_second_squared_t maxCentripetalAcceleration)
-    : m_maxCentripetalAcceleration(maxCentripetalAcceleration) {}
-
-units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t velocity) {
-  // ac = v^2 / r
-  // k (curvature) = 1 / r
-
-  // therefore, ac = v^2 * k
-  // ac / k = v^2
-  // v = std::sqrt(ac / k)
-
-  // We have to multiply by 1_rad here to get the units to cancel out nicely.
-  // The units library defines a unit for radians although it is technically
-  // unitless.
-  return units::math::sqrt(m_maxCentripetalAcceleration /
-                           units::math::abs(curvature) * 1_rad);
-}
-
-TrajectoryConstraint::MinMax
-CentripetalAccelerationConstraint::MinMaxAcceleration(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t speed) {
-  // The acceleration of the robot has no impact on the centripetal acceleration
-  // of the robot.
-  return {};
-}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
deleted file mode 100644
index 8b88bf4..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.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 "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
-
-using namespace frc;
-
-DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
-    DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
-    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
-
-units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t velocity) {
-  auto wheelSpeeds =
-      m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
-  wheelSpeeds.Normalize(m_maxSpeed);
-
-  return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
-}
-
-TrajectoryConstraint::MinMax
-DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
-    const Pose2d& pose, curvature_t curvature,
-    units::meters_per_second_t speed) {
-  return {};
-}
diff --git a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
deleted file mode 100644
index 3d6b61c..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
+++ /dev/null
@@ -1,79 +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/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
deleted file mode 100644
index 2fd8151..0000000
--- a/wpilibc/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.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 "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 eaf4ddc..5f34e77 100644
--- a/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -17,6 +17,7 @@
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <networktables/NetworkTableInstance.h>
+#include <wpimath/MathShared.h>
 
 #include "WPILibVersion.h"
 #include "frc/DriverStation.h"
@@ -68,6 +69,47 @@
     return std::make_pair(RobotBase::GetThreadId(), true);
   }
 };
+class WPILibMathShared : public wpi::math::MathShared {
+ public:
+  void ReportError(const wpi::Twine& error) override {
+    DriverStation::ReportError(error);
+  }
+
+  void ReportUsage(wpi::math::MathUsageId id, int count) override {
+    switch (id) {
+      case wpi::math::MathUsageId::kKinematics_DifferentialDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_DifferentialDrive);
+        break;
+      case wpi::math::MathUsageId::kKinematics_MecanumDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_MecanumDrive);
+        break;
+      case wpi::math::MathUsageId::kKinematics_SwerveDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Kinematics,
+                   HALUsageReporting::kKinematics_SwerveDrive);
+        break;
+      case wpi::math::MathUsageId::kTrajectory_TrapezoidProfile:
+        HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, count);
+        break;
+      case wpi::math::MathUsageId::kFilter_Linear:
+        HAL_Report(HALUsageReporting::kResourceType_LinearFilter, count);
+        break;
+      case wpi::math::MathUsageId::kOdometry_DifferentialDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_DifferentialDrive);
+        break;
+      case wpi::math::MathUsageId::kOdometry_SwerveDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_SwerveDrive);
+        break;
+      case wpi::math::MathUsageId::kOdometry_MecanumDrive:
+        HAL_Report(HALUsageReporting::kResourceType_Odometry,
+                   HALUsageReporting::kOdometry_MecanumDrive);
+        break;
+    }
+  }
+};
 }  // namespace
 
 static void SetupCameraServerShared() {
@@ -88,8 +130,6 @@
   if (symbol) {
     auto setCameraServerShared = (SetCameraServerSharedFP)symbol;
     setCameraServerShared(new WPILibCameraServerShared{});
-    wpi::outs() << "Set Camera Server Shared\n";
-    wpi::outs().flush();
   } else {
     wpi::outs() << "Camera Server Shared Symbol Missing\n";
     wpi::outs().flush();
@@ -103,14 +143,27 @@
 #endif
 }
 
+static void SetupMathShared() {
+  wpi::math::MathSharedStore::SetMathShared(
+      std::make_unique<WPILibMathShared>());
+}
+
 bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
 
 bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
 
 bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); }
 
+bool RobotBase::IsAutonomousEnabled() const {
+  return m_ds.IsAutonomousEnabled();
+}
+
 bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); }
 
+bool RobotBase::IsOperatorControlEnabled() const {
+  return m_ds.IsOperatorControlEnabled();
+}
+
 bool RobotBase::IsTest() const { return m_ds.IsTest(); }
 
 bool RobotBase::IsNewDataAvailable() const { return m_ds.IsNewControlData(); }
@@ -121,6 +174,7 @@
   m_threadId = std::this_thread::get_id();
 
   SetupCameraServerShared();
+  SetupMathShared();
 
   auto inst = nt::NetworkTableInstance::GetDefault();
   inst.SetNetworkIdentity("Robot");
diff --git a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h b/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
index ccdb75c..309fab3 100644
--- a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
+++ b/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 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.                                                               */
@@ -27,6 +27,7 @@
  * determine the heading.
  *
  * This class is for the digital ADXRS450 gyro sensor that connects via SPI.
+ * Only one instance of an ADXRS Gyro is supported.
  */
 class ADXRS450_Gyro : public GyroBase {
  public:
@@ -50,21 +51,19 @@
   /**
    * Return the actual angle in degrees that the robot is currently facing.
    *
-   * The angle is based on the current accumulator value corrected by the
-   * oversampling rate, the gyro type and the A/D calibration values.
+   * The angle is based on integration of the returned rate from the gyro.
    * The angle is continuous, that is it will continue from 360->361 degrees.
    * This allows algorithms that wouldn't want to see a discontinuity in the
    * gyro output as it sweeps from 360 to 0 on the second time around.
    *
-   * @return the current heading of the robot in degrees. This heading is based
-   *         on integration of the returned rate from the gyro.
+   * @return the current heading of the robot in degrees.
    */
   double GetAngle() const override;
 
   /**
    * Return the rate of rotation of the gyro
    *
-   * The rate is based on the most recent reading of the gyro analog value
+   * The rate is based on the most recent reading of the gyro.
    *
    * @return the current rate in degrees per second
    */
@@ -93,8 +92,16 @@
    */
   void Calibrate() override;
 
+  /**
+   * Get the SPI port number.
+   *
+   * @return The SPI port number.
+   */
+  int GetPort() const;
+
  private:
   SPI m_spi;
+  SPI::Port m_port;
 
   hal::SimDevice m_simDevice;
   hal::SimDouble m_simAngle;
diff --git a/wpilibc/src/main/native/include/frc/AddressableLED.h b/wpilibc/src/main/native/include/frc/AddressableLED.h
index 5534ae4..3bcbab5 100644
--- a/wpilibc/src/main/native/include/frc/AddressableLED.h
+++ b/wpilibc/src/main/native/include/frc/AddressableLED.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -11,7 +11,7 @@
 
 #include <hal/AddressableLEDTypes.h>
 #include <hal/Types.h>
-#include <units/units.h>
+#include <units/time.h>
 #include <wpi/ArrayRef.h>
 
 #include "frc/ErrorBase.h"
@@ -22,6 +22,8 @@
 
 /**
  * A class for driving addressable LEDs, such as WS2812s and NeoPixels.
+ *
+ * <p>Only 1 LED driver is currently supported by the roboRIO.
  */
 class AddressableLED : public ErrorBase {
  public:
@@ -83,7 +85,7 @@
   /**
    * Constructs a new driver for a specific port.
    *
-   * @param port the output port to use (Must be a PWM port)
+   * @param port the output port to use (Must be a PWM header)
    */
   explicit AddressableLED(int port);
 
@@ -95,6 +97,8 @@
    * <p>Calling this is an expensive call, so its best to call it once, then
    * just update data.
    *
+   * <p>The max length is 5460 LEDs.
+   *
    * @param length the strip length
    */
   void SetLength(int length);
@@ -147,7 +151,7 @@
   /**
    * Starts the output.
    *
-   * <p>The output writes continously.
+   * <p>The output writes continuously.
    */
   void Start();
 
diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
index 872db87..3dc22ea 100644
--- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h
+++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -11,7 +11,7 @@
 
 #include <hal/SimDevice.h>
 #include <hal/Types.h>
-#include <units/units.h>
+#include <units/angle.h>
 
 #include "frc/AnalogTrigger.h"
 #include "frc/Counter.h"
@@ -108,6 +108,13 @@
    */
   double GetDistance() const;
 
+  /**
+   * Get the channel number.
+   *
+   * @return The channel number.
+   */
+  int GetChannel() const;
+
   void InitSendable(SendableBuilder& builder) override;
 
  private:
diff --git a/wpilibc/src/main/native/include/frc/AnalogGyro.h b/wpilibc/src/main/native/include/frc/AnalogGyro.h
index bcc67c9..b59df5b 100644
--- a/wpilibc/src/main/native/include/frc/AnalogGyro.h
+++ b/wpilibc/src/main/native/include/frc/AnalogGyro.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -186,6 +186,13 @@
 
   void Calibrate() override;
 
+  /**
+   * Gets the analog input for the gyro.
+   *
+   * @return AnalogInput
+   */
+  std::shared_ptr<AnalogInput> GetAnalogInput() const;
+
  protected:
   std::shared_ptr<AnalogInput> m_analog;
 
diff --git a/wpilibc/src/main/native/include/frc/AnalogOutput.h b/wpilibc/src/main/native/include/frc/AnalogOutput.h
index 1cecd70..026986b 100644
--- a/wpilibc/src/main/native/include/frc/AnalogOutput.h
+++ b/wpilibc/src/main/native/include/frc/AnalogOutput.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 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.                                                               */
@@ -55,7 +55,7 @@
   /**
    * Get the channel of this AnalogOutput.
    */
-  int GetChannel();
+  int GetChannel() const;
 
   void InitSendable(SendableBuilder& builder) override;
 
diff --git a/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h b/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
index 446a920..ed943f7 100644
--- a/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
+++ b/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 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.                                                               */
@@ -42,11 +42,12 @@
    * This will calculate the result from the fullRange times the fraction of the
    * supply voltage, plus the offset.
    *
-   * @param channel   The channel number on the roboRIO to represent. 0-3 are
-   *                  on-board 4-7 are on the MXP port.
-   * @param fullRange The angular value (in desired units) representing the full
+   * @param channel   The Analog Input channel number on the roboRIO the
+   *                  potentiometer is plugged into. 0-3 are on-board and 4-7
+   *                  are on the MXP port.
+   * @param fullRange The value (in desired units) representing the full
    *                  0-5V range of the input.
-   * @param offset    The angular value (in desired units) representing the
+   * @param offset    The value (in desired units) representing the
    *                  angular output at 0V.
    */
   explicit AnalogPotentiometer(int channel, double fullRange = 1.0,
@@ -66,9 +67,9 @@
    * supply voltage, plus the offset.
    *
    * @param channel   The existing Analog Input pointer
-   * @param fullRange The angular value (in desired units) representing the full
+   * @param fullRange The value (in desired units) representing the full
    *                  0-5V range of the input.
-   * @param offset    The angular value (in desired units) representing the
+   * @param offset    The value (in desired units) representing the
    *                  angular output at 0V.
    */
   explicit AnalogPotentiometer(AnalogInput* input, double fullRange = 1.0,
@@ -88,9 +89,9 @@
    * supply voltage, plus the offset.
    *
    * @param channel   The existing Analog Input pointer
-   * @param fullRange The angular value (in desired units) representing the full
+   * @param fullRange The value (in desired units) representing the full
    *                  0-5V range of the input.
-   * @param offset    The angular value (in desired units) representing the
+   * @param offset    The value (in desired units) representing the
    *                  angular output at 0V.
    */
   explicit AnalogPotentiometer(std::shared_ptr<AnalogInput> input,
@@ -102,7 +103,7 @@
   AnalogPotentiometer& operator=(AnalogPotentiometer&&) = default;
 
   /**
-   * Get the current reading of the potentiomer.
+   * Get the current reading of the potentiometer.
    *
    * @return The current position of the potentiometer (in the units used for
    *         fullRange and offset).
diff --git a/wpilibc/src/main/native/include/frc/AnalogTrigger.h b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
index 7554bd9..d465652 100644
--- a/wpilibc/src/main/native/include/frc/AnalogTrigger.h
+++ b/wpilibc/src/main/native/include/frc/AnalogTrigger.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -101,17 +101,6 @@
   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.
diff --git a/wpilibc/src/main/native/include/frc/CAN.h b/wpilibc/src/main/native/include/frc/CAN.h
index ed861fb..612d533 100644
--- a/wpilibc/src/main/native/include/frc/CAN.h
+++ b/wpilibc/src/main/native/include/frc/CAN.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 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,6 @@
 #include <stdint.h>
 
 #include <hal/CANAPITypes.h>
-#include <wpi/ArrayRef.h>
 
 #include "frc/ErrorBase.h"
 
@@ -94,6 +93,38 @@
   void WriteRTRFrame(int length, int apiId);
 
   /**
+   * Write a packet to the CAN device with a specific ID. This ID is 10 bits.
+   *
+   * @param data The data to write (8 bytes max)
+   * @param length The data length to write
+   * @param apiId The API ID to write.
+   */
+  int WritePacketNoError(const uint8_t* data, int length, int apiId);
+
+  /**
+   * Write a repeating packet to the CAN device with a specific ID. This ID is
+   * 10 bits. The RoboRIO will automatically repeat the packet at the specified
+   * interval
+   *
+   * @param data The data to write (8 bytes max)
+   * @param length The data length to write
+   * @param apiId The API ID to write.
+   * @param repeatMs The period to repeat the packet at.
+   */
+  int WritePacketRepeatingNoError(const uint8_t* data, int length, int apiId,
+                                  int repeatMs);
+
+  /**
+   * Write an RTR frame to the CAN device with a specific ID. This ID is 10
+   * bits. The length by spec must match what is returned by the responding
+   * device
+   *
+   * @param length The length to request (0 to 8)
+   * @param apiId The API ID to write.
+   */
+  int WriteRTRFrameNoError(int length, int apiId);
+
+  /**
    * Stop a repeating packet with a specific ID. This ID is 10 bits.
    *
    * @param apiId The API ID to stop repeating
diff --git a/wpilibc/src/main/native/include/frc/Compressor.h b/wpilibc/src/main/native/include/frc/Compressor.h
index a5e512c..d1a4dca 100644
--- a/wpilibc/src/main/native/include/frc/Compressor.h
+++ b/wpilibc/src/main/native/include/frc/Compressor.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 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.                                                               */
@@ -171,6 +171,13 @@
    */
   void ClearAllPCMStickyFaults();
 
+  /**
+   * Gets module number (CAN ID).
+   *
+   * @return Module number
+   */
+  int GetModule() const;
+
   void InitSendable(SendableBuilder& builder) override;
 
  protected:
diff --git a/wpilibc/src/main/native/include/frc/Controller.h b/wpilibc/src/main/native/include/frc/Controller.h
index 4124046..c6b25ab 100644
--- a/wpilibc/src/main/native/include/frc/Controller.h
+++ b/wpilibc/src/main/native/include/frc/Controller.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -17,10 +17,12 @@
  * Common interface for controllers. Controllers run control loops, the most
  * common are PID controllers and their variants, but this includes anything
  * that is controlling an actuator in a separate thread.
+ *
+ * @deprecated Only used by the deprecated PIDController
  */
 class Controller {
  public:
-  WPI_DEPRECATED("None of the 2020 FRC controllers use this.")
+  WPI_DEPRECATED("Only used by the deprecated PIDController")
   Controller() = default;
   virtual ~Controller() = default;
 
diff --git a/wpilibc/src/main/native/include/frc/DMASample.h b/wpilibc/src/main/native/include/frc/DMASample.h
index 4592930..c286c8d 100644
--- a/wpilibc/src/main/native/include/frc/DMASample.h
+++ b/wpilibc/src/main/native/include/frc/DMASample.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -11,7 +11,7 @@
 
 #include <hal/AnalogInput.h>
 #include <hal/DMA.h>
-#include <units/units.h>
+#include <units/time.h>
 
 #include "frc/AnalogInput.h"
 #include "frc/Counter.h"
diff --git a/wpilibc/src/main/native/include/frc/DoubleSolenoid.h b/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
index 6722976..302c52c 100644
--- a/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
+++ b/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -69,6 +69,15 @@
   virtual Value Get() const;
 
   /**
+   * Toggle the value of the solenoid.
+   *
+   * If the solenoid is set to forward, it'll be set to reverse. If the solenoid
+   * is set to reverse, it'll be set to forward. If the solenoid is set to off,
+   * nothing happens.
+   */
+  void Toggle();
+
+  /**
    * Check if the forward solenoid is blacklisted.
    *
    * If a solenoid is shorted, it is added to the blacklist and disabled until
diff --git a/wpilibc/src/main/native/include/frc/DriverStation.h b/wpilibc/src/main/native/include/frc/DriverStation.h
index 4508701..a6c40d6 100644
--- a/wpilibc/src/main/native/include/frc/DriverStation.h
+++ b/wpilibc/src/main/native/include/frc/DriverStation.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,11 +16,9 @@
 #include <hal/DriverStationTypes.h>
 #include <wpi/Twine.h>
 #include <wpi/condition_variable.h>
-#include <wpi/deprecated.h>
 #include <wpi/mutex.h>
 
 #include "frc/ErrorBase.h"
-#include "frc/RobotState.h"
 
 namespace frc {
 
@@ -184,6 +182,17 @@
   int GetJoystickAxisType(int stick, int axis) const;
 
   /**
+   * Returns if a joystick is connected to the Driver Station.
+   *
+   * This makes a best effort guess by looking at the reported number of axis,
+   * buttons, and POVs attached.
+   *
+   * @param stick The joystick port number
+   * @return true if a joystick is connected
+   */
+  bool IsJoystickConnected(int stick) const;
+
+  /**
    * Check if the DS has enabled the robot.
    *
    * @return True if the robot is enabled and the DS is connected
@@ -212,6 +221,15 @@
   bool IsAutonomous() const;
 
   /**
+   * Check if the DS is commanding autonomous mode and if it has enabled the
+   * robot.
+   *
+   * @return True if the robot is being commanded to be in autonomous mode and
+   * enabled.
+   */
+  bool IsAutonomousEnabled() const;
+
+  /**
    * Check if the DS is commanding teleop mode.
    *
    * @return True if the robot is being commanded to be in teleop mode
@@ -219,6 +237,14 @@
   bool IsOperatorControl() const;
 
   /**
+   * Check if the DS is commanding teleop mode and if it has enabled the robot.
+   *
+   * @return True if the robot is being commanded to be in teleop mode and
+   * enabled.
+   */
+  bool IsOperatorControlEnabled() const;
+
+  /**
    * Check if the DS is commanding test mode.
    *
    * @return True if the robot is being commanded to be in test mode
@@ -313,6 +339,9 @@
    *
    * This is a good way to delay processing until there is new driver station
    * data to act on.
+   *
+   * Checks if new control data has arrived since the last waitForData call
+   * on the current thread. If new data has not arrived, returns immediately.
    */
   void WaitForData();
 
@@ -320,6 +349,9 @@
    * Wait until a new packet comes from the driver station, or wait for a
    * timeout.
    *
+   * Checks if new control data has arrived since the last waitForData call
+   * on the current thread. If new data has not arrived, returns immediately.
+   *
    * If the timeout is less then or equal to 0, wait indefinitely.
    *
    * Timeout is in milliseconds
@@ -345,7 +377,7 @@
    * Warning: This is not an official time (so it cannot be used to dispute ref
    * calls or guarantee that a function will trigger before the match ends).
    *
-   * The Practice Match function of the DS approximates the behaviour seen on
+   * The Practice Match function of the DS approximates the behavior seen on
    * the field.
    *
    * @return Time remaining in current match period (auto or teleop)
@@ -399,6 +431,23 @@
    */
   void WakeupWaitForData();
 
+  /**
+   * Allows the user to specify whether they want joystick connection warnings
+   * to be printed to the console. This setting is ignored when the FMS is
+   * connected -- warnings will always be on in that scenario.
+   *
+   * @param silence Whether warning messages should be silenced.
+   */
+  void SilenceJoystickConnectionWarning(bool silence);
+
+  /**
+   * Returns whether joystick connection warnings are silenced. This will
+   * always return false when connected to the FMS.
+   *
+   * @return Whether joystick connection warnings are silenced.
+   */
+  bool IsJoystickConnectionWarningSilenced() const;
+
  protected:
   /**
    * Copy data from the DS task for the user.
@@ -446,10 +495,12 @@
   std::thread m_dsThread;
   std::atomic<bool> m_isRunning{false};
 
-  wpi::mutex m_waitForDataMutex;
+  mutable wpi::mutex m_waitForDataMutex;
   wpi::condition_variable m_waitForDataCond;
   int m_waitForDataCounter;
 
+  bool m_silenceJoystickWarning = false;
+
   // Robot state status variables
   bool m_userInDisabled = false;
   bool m_userInAutonomous = false;
diff --git a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
index 92864a8..17b038f 100644
--- a/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
+++ b/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -11,7 +11,7 @@
 
 #include <hal/SimDevice.h>
 #include <hal/Types.h>
-#include <units/units.h>
+#include <units/angle.h>
 
 #include "frc/AnalogTrigger.h"
 #include "frc/Counter.h"
@@ -154,14 +154,28 @@
    */
   double GetDistance() const;
 
+  /**
+   * Get the FPGA index for the DutyCycleEncoder.
+   *
+   * @return the FPGA index
+   */
+  int GetFPGAIndex() const;
+
+  /**
+   * Get the channel of the source.
+   *
+   * @return the source channel
+   */
+  int GetSourceChannel() const;
+
   void InitSendable(SendableBuilder& builder) override;
 
  private:
   void Init();
 
   std::shared_ptr<DutyCycle> m_dutyCycle;
-  AnalogTrigger m_analogTrigger;
-  Counter m_counter;
+  std::unique_ptr<AnalogTrigger> m_analogTrigger;
+  std::unique_ptr<Counter> m_counter;
   int m_frequencyThreshold = 100;
   double m_positionOffset = 0;
   double m_distancePerRotation = 1.0;
@@ -169,6 +183,7 @@
 
   hal::SimDevice m_simDevice;
   hal::SimDouble m_simPosition;
+  hal::SimDouble m_simDistancePerRotation;
   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 60552f6..76f9d1c 100644
--- a/wpilibc/src/main/native/include/frc/Encoder.h
+++ b/wpilibc/src/main/native/include/frc/Encoder.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -192,6 +192,9 @@
    *                  the FPGA will report the device stopped. This is expressed
    *                  in seconds.
    */
+  WPI_DEPRECATED(
+      "Use SetMinRate() in favor of this method.  This takes unscaled periods "
+      "and SetMinRate() scales using value from SetDistancePerPulse().")
   void SetMaxPeriod(double maxPeriod) override;
 
   /**
diff --git a/wpilibc/src/main/native/include/frc/Error.h b/wpilibc/src/main/native/include/frc/Error.h
index 8eafd10..d63fb62 100644
--- a/wpilibc/src/main/native/include/frc/Error.h
+++ b/wpilibc/src/main/native/include/frc/Error.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,8 +7,6 @@
 
 #pragma once
 
-#include <stdint.h>
-
 #include <string>
 
 #include <wpi/StringRef.h>
@@ -19,8 +17,6 @@
 #undef GetMessage
 #endif
 
-#include "frc/Base.h"
-
 namespace frc {
 
 class ErrorBase;
diff --git a/wpilibc/src/main/native/include/frc/ErrorBase.h b/wpilibc/src/main/native/include/frc/ErrorBase.h
index e9168aa..0ced9a2 100644
--- a/wpilibc/src/main/native/include/frc/ErrorBase.h
+++ b/wpilibc/src/main/native/include/frc/ErrorBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -11,9 +11,7 @@
 
 #include <wpi/StringRef.h>
 #include <wpi/Twine.h>
-#include <wpi/mutex.h>
 
-#include "frc/Base.h"
 #include "frc/Error.h"
 
 // Forward declared manually to avoid needing to pull in entire HAL header.
diff --git a/wpilibc/src/main/native/include/frc/Filesystem.h b/wpilibc/src/main/native/include/frc/Filesystem.h
index b7ef3f1..f196f7a 100644
--- a/wpilibc/src/main/native/include/frc/Filesystem.h
+++ b/wpilibc/src/main/native/include/frc/Filesystem.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 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.                                                               */
@@ -34,7 +34,7 @@
  * Obtains the deploy directory of the program, which is the remote location
  * src/main/deploy is deployed to by default. On the roboRIO, this is
  * /home/lvuser/deploy. In simulation, it is where the simulation was launched
- * from, in the subdirectory "deploy" (`pwd`/deploy).
+ * from, in the subdirectory "src/main/deploy" (`pwd`/src/main/deploy).
  *
  * @param result The result of the operating directory lookup
  */
diff --git a/wpilibc/src/main/native/include/frc/GearTooth.h b/wpilibc/src/main/native/include/frc/GearTooth.h
index 1c3df5b..2d1f792 100644
--- a/wpilibc/src/main/native/include/frc/GearTooth.h
+++ b/wpilibc/src/main/native/include/frc/GearTooth.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -22,6 +22,8 @@
  * Implements the gear tooth sensor supplied by FIRST. Currently there is no
  * reverse sensing on the gear tooth sensor, but in future versions we might
  * implement the necessary timing in the FPGA to sense direction.
+ *
+ * @deprecated No longer used per FMS usage reporting
  */
 class GearTooth : public Counter {
  public:
diff --git a/wpilibc/src/main/native/include/frc/GenericHID.h b/wpilibc/src/main/native/include/frc/GenericHID.h
index aae10cf..001b984 100644
--- a/wpilibc/src/main/native/include/frc/GenericHID.h
+++ b/wpilibc/src/main/native/include/frc/GenericHID.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -62,6 +62,9 @@
    * the state of each button. The appropriate button is returned as a boolean
    * value.
    *
+   * This method returns true if the button is being held down at the time
+   * that this method is being called.
+   *
    * @param button The button number to be read (starting at 1)
    * @return The state of the button.
    */
@@ -71,6 +74,10 @@
    * Whether the button was pressed since the last check. Button indexes begin
    * at 1.
    *
+   * This method returns true if the button went from not pressed to held down
+   * since the last time this method was called. This is useful if you only
+   * want to call a function once when you press the button.
+   *
    * @param button The button index, beginning at 1.
    * @return Whether the button was pressed since the last check.
    */
@@ -80,6 +87,10 @@
    * Whether the button was released since the last check. Button indexes begin
    * at 1.
    *
+   * This method returns true if the button went from held down to not pressed
+   * since the last time this method was called. This is useful if you only
+   * want to call a function once when you release the button.
+   *
    * @param button The button index, beginning at 1.
    * @return Whether the button was released since the last check.
    */
@@ -126,6 +137,13 @@
   int GetButtonCount() const;
 
   /**
+   * Get if the HID is connected.
+   *
+   * @return true if the HID is connected
+   */
+  bool IsConnected() const;
+
+  /**
    * Get the type of the HID.
    *
    * @return the type of the HID.
diff --git a/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h b/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h
index 8c2a564..42b7434 100644
--- a/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h
+++ b/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -115,9 +115,8 @@
   /**
    * Return the timestamp for the rising interrupt that occurred most recently.
    *
-   * This is in the same time domain as GetClock().
-   * The rising-edge interrupt should be enabled with
-   * {@link #DigitalInput.SetUpSourceEdge}
+   * This is in the same time domain as GetClock(). The rising-edge interrupt
+   * should be enabled with SetUpSourceEdge().
    *
    * @return Timestamp in seconds since boot.
    */
diff --git a/wpilibc/src/main/native/include/frc/IterativeRobot.h b/wpilibc/src/main/native/include/frc/IterativeRobot.h
index 447c477..24fdba3 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-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -21,6 +21,9 @@
  *
  * Periodic() functions from the base class are called each time a new packet is
  * received from the driver station.
+ *
+ * @deprecated Use TimedRobot instead. It's a drop-in replacement that provides
+ * more regular execution periods.
  */
 class IterativeRobot : public IterativeRobotBase {
  public:
@@ -33,7 +36,7 @@
   /**
    * Provide an alternate "main loop" via StartCompetition().
    *
-   * This specific StartCompetition() implements "main loop" behaviour synced
+   * This specific StartCompetition() implements "main loop" behavior synced
    * with the DS packets.
    */
   void StartCompetition() override;
diff --git a/wpilibc/src/main/native/include/frc/IterativeRobotBase.h b/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
index b78765a..66897aa 100644
--- a/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
+++ b/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 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 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/time.h>
 #include <wpi/deprecated.h>
 
 #include "frc/RobotBase.h"
@@ -62,6 +62,16 @@
   virtual void RobotInit();
 
   /**
+   * Robot-wide simulation initialization code should go here.
+   *
+   * Users should override this method for default Robot-wide simulation
+   * related initialization which will be called when the robot is first
+   * started. It will be called exactly one time after RobotInit is called
+   * only when the robot is in simulation.
+   */
+  virtual void SimulationInit();
+
+  /**
    * Initialization code for disabled mode should go here.
    *
    * Users should override this method for initialization code which will be
@@ -103,6 +113,13 @@
   virtual void RobotPeriodic();
 
   /**
+   * Periodic simulation code should go here.
+   *
+   * This function is called in a simulated robot after user code executes.
+   */
+  virtual void SimulationPeriodic();
+
+  /**
    * Periodic code for disabled mode should go here.
    *
    * Users should override this method for code which will be called each time a
@@ -138,13 +155,15 @@
    */
   virtual void TestPeriodic();
 
- protected:
   /**
    * Constructor for IterativeRobotBase.
    *
    * @param period Period in seconds.
+   *
+   * @deprecated Use IterativeRobotBase(units::second_t period) with unit-safety
+   * instead
    */
-  WPI_DEPRECATED("Use ctor with unit-safety instead.")
+  WPI_DEPRECATED("Use constructor with unit-safety instead.")
   explicit IterativeRobotBase(double period);
 
   /**
@@ -156,6 +175,7 @@
 
   virtual ~IterativeRobotBase() = default;
 
+ protected:
   IterativeRobotBase(IterativeRobotBase&&) = default;
   IterativeRobotBase& operator=(IterativeRobotBase&&) = default;
 
diff --git a/wpilibc/src/main/native/include/frc/Joystick.h b/wpilibc/src/main/native/include/frc/Joystick.h
index 4ae398c..0975e6d 100644
--- a/wpilibc/src/main/native/include/frc/Joystick.h
+++ b/wpilibc/src/main/native/include/frc/Joystick.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,8 +9,6 @@
 
 #include <array>
 
-#include <wpi/deprecated.h>
-
 #include "frc/GenericHID.h"
 
 namespace frc {
diff --git a/wpilibc/src/main/native/include/frc/LinearFilter.h b/wpilibc/src/main/native/include/frc/LinearFilter.h
deleted file mode 100644
index 2b83a32..0000000
--- a/wpilibc/src/main/native/include/frc/LinearFilter.h
+++ /dev/null
@@ -1,194 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#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>
-
-namespace frc {
-
-/**
- * This class implements a linear, digital filter. All types of FIR and IIR
- * filters are supported. Static factory methods are provided to create commonly
- * used types of filters.
- *
- * Filters are of the form:<br>
- *  y[n] = (b0 * x[n] + b1 * x[n-1] + … + bP * x[n-P]) -
- *         (a0 * y[n-1] + a2 * y[n-2] + … + aQ * y[n-Q])
- *
- * Where:<br>
- *  y[n] is the output at time "n"<br>
- *  x[n] is the input at time "n"<br>
- *  y[n-1] is the output from the LAST time step ("n-1")<br>
- *  x[n-1] is the input from the LAST time step ("n-1")<br>
- *  b0 … bP are the "feedforward" (FIR) gains<br>
- *  a0 … aQ are the "feedback" (IIR) gains<br>
- * IMPORTANT! Note the "-" sign in front of the feedback term! This is a common
- *            convention in signal processing.
- *
- * What can linear filters do? Basically, they can filter, or diminish, the
- * effects of undesirable input frequencies. High frequencies, or rapid changes,
- * can be indicative of sensor noise or be otherwise undesirable. A "low pass"
- * filter smooths out the signal, reducing the impact of these high frequency
- * components.  Likewise, a "high pass" filter gets rid of slow-moving signal
- * components, letting you detect large changes more easily.
- *
- * Example FRC applications of filters:
- *  - Getting rid of noise from an analog sensor input (note: the roboRIO's FPGA
- *    can do this faster in hardware)
- *  - Smoothing out joystick input to prevent the wheels from slipping or the
- *    robot from tipping
- *  - Smoothing motor commands so that unnecessary strain isn't put on
- *    electrical or mechanical components
- *  - If you use clever gains, you can make a PID controller out of this class!
- *
- * For more on filters, we highly recommend the following articles:<br>
- * https://en.wikipedia.org/wiki/Linear_filter<br>
- * https://en.wikipedia.org/wiki/Iir_filter<br>
- * https://en.wikipedia.org/wiki/Fir_filter<br>
- *
- * Note 1: Calculate() should be called by the user on a known, regular period.
- * You can use a Notifier for this or do it "inline" with code in a
- * periodic function.
- *
- * Note 2: For ALL filters, gains are necessarily a function of frequency. If
- * you make a filter that works well for you at, say, 100Hz, you will most
- * definitely need to adjust the gains if you then want to run it at 200Hz!
- * 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:
-  /**
-   * Create a linear FIR or IIR filter.
-   *
-   * @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)
-      : 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.
-   *
-   * @param ffGains The "feed forward" or FIR gains.
-   * @param fbGains The "feed back" or IIR gains.
-   */
-  LinearFilter(std::initializer_list<double> ffGains,
-               std::initializer_list<double> fbGains)
-      : LinearFilter(wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
-                     wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
-
-  // Static methods to create commonly used filters
-  /**
-   * Creates a one-pole IIR low-pass filter of the form:<br>
-   *   y[n] = (1 - gain) * x[n] + gain * y[n-1]<br>
-   * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
-   *
-   * This filter is stable for time constants greater than zero.
-   *
-   * @param timeConstant The discrete-time time constant in seconds.
-   * @param period       The period in seconds between samples taken by the
-   *                     user.
-   */
-  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>
-   *   y[n] = gain * x[n] + (-gain) * x[n-1] + gain * y[n-1]<br>
-   * where gain = e<sup>-dt / T</sup>, T is the time constant in seconds
-   *
-   * This filter is stable for time constants greater than zero.
-   *
-   * @param timeConstant The discrete-time time constant in seconds.
-   * @param period       The period in seconds between samples taken by the
-   *                     user.
-   */
-  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>
-   *   y[n] = 1/k * (x[k] + x[k-1] + … + x[0])
-   *
-   * This filter is always stable.
-   *
-   * @param taps The number of samples to average over. Higher = smoother but
-   *             slower
-   */
-  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() {
-    m_inputs.reset();
-    m_outputs.reset();
-  }
-
-  /**
-   * Calculates the next value of the filter.
-   *
-   * @param input Current input value.
-   *
-   * @return The filtered value at this step
-   */
-  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<T> m_inputs;
-  wpi::circular_buffer<T> m_outputs;
-  std::vector<double> m_inputGains;
-  std::vector<double> m_outputGains;
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/MedianFilter.h b/wpilibc/src/main/native/include/frc/MedianFilter.h
deleted file mode 100644
index b5d499b..0000000
--- a/wpilibc/src/main/native/include/frc/MedianFilter.h
+++ /dev/null
@@ -1,80 +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 <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 24ffba3..c9348a6 100644
--- a/wpilibc/src/main/native/include/frc/Notifier.h
+++ b/wpilibc/src/main/native/include/frc/Notifier.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,10 +12,11 @@
 #include <atomic>
 #include <functional>
 #include <thread>
+#include <type_traits>
 #include <utility>
 
 #include <hal/Types.h>
-#include <units/units.h>
+#include <units/time.h>
 #include <wpi/Twine.h>
 #include <wpi/deprecated.h>
 #include <wpi/mutex.h>
@@ -34,12 +35,34 @@
    */
   explicit Notifier(std::function<void()> handler);
 
-  template <typename Callable, typename Arg, typename... Args>
+  template <
+      typename Callable, typename Arg, typename... Args,
+      typename = std::enable_if_t<std::is_invocable_v<Callable, Arg, Args...>>>
   Notifier(Callable&& f, Arg&& arg, Args&&... args)
       : Notifier(std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
                            std::forward<Args>(args)...)) {}
 
   /**
+   * Create a Notifier for timer event notification.
+   *
+   * This overload makes the underlying thread run with a real-time priority.
+   * This is useful for reducing scheduling jitter on processes which are
+   * sensitive to timing variance, like model-based control.
+   *
+   * @param priority The FIFO real-time scheduler priority ([0..100] where a
+   *                 lower number represents higher priority).
+   * @param handler  The handler is called at the notification time which is set
+   *                 using StartSingle or StartPeriodic.
+   */
+  explicit Notifier(int priority, std::function<void()> handler);
+
+  template <typename Callable, typename Arg, typename... Args>
+  Notifier(int priority, Callable&& f, Arg&& arg, Args&&... args)
+      : Notifier(priority,
+                 std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
+                           std::forward<Args>(args)...)) {}
+
+  /**
    * Free the resources for a timer event.
    */
   virtual ~Notifier();
@@ -66,6 +89,9 @@
    *
    * A timer event is queued for a single event after the specified delay.
    *
+   * @deprecated Use unit-safe StartSingle(units::second_t delay) method
+   * instead.
+   *
    * @param delay Seconds to wait before the handler is called.
    */
   WPI_DEPRECATED("Use unit-safe StartSingle method instead.")
@@ -87,6 +113,9 @@
    * interrupt occurs, the event will be immediately requeued for the same time
    * interval.
    *
+   * @deprecated Use unit-safe StartPeriodic(units::second_t period) method
+   * instead
+   *
    * @param period Period in seconds to call the handler starting one period
    *               after the call to this method.
    */
@@ -106,9 +135,9 @@
   void StartPeriodic(units::second_t period);
 
   /**
-   * Stop timer events from occuring.
+   * Stop timer events from occurring.
    *
-   * Stop any repeating timer events from occuring. This will also remove any
+   * Stop any repeating timer events from occurring. This will also remove any
    * single notification events from the queue.
    *
    * If a timer-based call to the registered handler is in progress, this
diff --git a/wpilibc/src/main/native/include/frc/PIDBase.h b/wpilibc/src/main/native/include/frc/PIDBase.h
index 513d46b..79d8eba 100644
--- a/wpilibc/src/main/native/include/frc/PIDBase.h
+++ b/wpilibc/src/main/native/include/frc/PIDBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -35,6 +35,8 @@
  * This feedback controller runs in discrete time, so time deltas are not used
  * in the integral and derivative calculations. Therefore, the sample rate
  * affects the controller's behavior for a given set of PID constants.
+ *
+ * @deprecated All APIs which use this have been deprecated.
  */
 class PIDBase : public PIDInterface,
                 public PIDOutput,
diff --git a/wpilibc/src/main/native/include/frc/PIDController.h b/wpilibc/src/main/native/include/frc/PIDController.h
index 88b0786..39e4deb 100644
--- a/wpilibc/src/main/native/include/frc/PIDController.h
+++ b/wpilibc/src/main/native/include/frc/PIDController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -33,6 +33,8 @@
  * This feedback controller runs in discrete time, so time deltas are not used
  * in the integral and derivative calculations. Therefore, the sample rate
  * affects the controller's behavior for a given set of PID constants.
+ *
+ * @deprecated Use frc2::PIDController class instead.
  */
 class PIDController : public PIDBase, public Controller {
  public:
@@ -46,7 +48,7 @@
    * @param output The PIDOutput object that is set to the output value
    * @param period the loop time for doing calculations in seconds. This
    *               particularly affects calculations of the integral and
-   *               differental terms. The default is 0.05 (50ms).
+   *               differential terms. The default is 0.05 (50ms).
    */
   WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, PIDSource* source,
@@ -62,7 +64,7 @@
    * @param output The PIDOutput object that is set to the output value
    * @param period the loop time for doing calculations in seconds. This
    *               particularly affects calculations of the integral and
-   *               differental terms. The default is 0.05 (50ms).
+   *               differential terms. The default is 0.05 (50ms).
    */
   WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, double f, PIDSource* source,
@@ -78,7 +80,7 @@
    * @param output The PIDOutput object that is set to the output value
    * @param period the loop time for doing calculations in seconds. This
    *               particularly affects calculations of the integral and
-   *               differental terms. The default is 0.05 (50ms).
+   *               differential terms. The default is 0.05 (50ms).
    */
   WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, PIDSource& source,
@@ -94,7 +96,7 @@
    * @param output The PIDOutput object that is set to the output value
    * @param period the loop time for doing calculations in seconds. This
    *               particularly affects calculations of the integral and
-   *               differental terms. The default is 0.05 (50ms).
+   *               differential terms. The default is 0.05 (50ms).
    */
   WPI_DEPRECATED("Use frc2::PIDController class instead.")
   PIDController(double p, double i, double d, double f, PIDSource& source,
diff --git a/wpilibc/src/main/native/include/frc/PIDInterface.h b/wpilibc/src/main/native/include/frc/PIDInterface.h
index 8162aa5..8d847a6 100644
--- a/wpilibc/src/main/native/include/frc/PIDInterface.h
+++ b/wpilibc/src/main/native/include/frc/PIDInterface.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 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.                                                               */
@@ -11,6 +11,11 @@
 
 namespace frc {
 
+/**
+ * Interface for PID Control Loop.
+ *
+ * @deprecated All APIs which use this have been deprecated.
+ */
 class PIDInterface {
  public:
   WPI_DEPRECATED("All APIs which use this have been deprecated.")
diff --git a/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h b/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h
index 433874b..9ec00a3 100644
--- a/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h
+++ b/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 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.                                                               */
@@ -85,10 +85,16 @@
    */
   void ClearStickyFaults();
 
+  /**
+   * Gets module number (CAN ID).
+   */
+  int GetModule() const;
+
   void InitSendable(SendableBuilder& builder) override;
 
  private:
   hal::Handle<HAL_PDPHandle> m_handle;
+  int m_module;
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Preferences.h b/wpilibc/src/main/native/include/frc/Preferences.h
index 57678a8..f04b012 100644
--- a/wpilibc/src/main/native/include/frc/Preferences.h
+++ b/wpilibc/src/main/native/include/frc/Preferences.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 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.                                                               */
@@ -122,6 +122,12 @@
   void PutString(wpi::StringRef key, wpi::StringRef value);
 
   /**
+   * Puts the given string into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitString(wpi::StringRef key, wpi::StringRef value);
+
+  /**
    * Puts the given int into the preferences table.
    *
    * The key may not have any whitespace nor an equals sign.
@@ -132,6 +138,12 @@
   void PutInt(wpi::StringRef key, int value);
 
   /**
+   * Puts the given int into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitInt(wpi::StringRef key, int value);
+
+  /**
    * Puts the given double into the preferences table.
    *
    * The key may not have any whitespace nor an equals sign.
@@ -142,6 +154,12 @@
   void PutDouble(wpi::StringRef key, double value);
 
   /**
+   * Puts the given double into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitDouble(wpi::StringRef key, double value);
+
+  /**
    * Puts the given float into the preferences table.
    *
    * The key may not have any whitespace nor an equals sign.
@@ -152,6 +170,12 @@
   void PutFloat(wpi::StringRef key, float value);
 
   /**
+   * Puts the given float into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitFloat(wpi::StringRef key, float value);
+
+  /**
    * Puts the given boolean into the preferences table.
    *
    * The key may not have any whitespace nor an equals sign.
@@ -162,6 +186,12 @@
   void PutBoolean(wpi::StringRef key, bool value);
 
   /**
+   * Puts the given boolean into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitBoolean(wpi::StringRef key, bool value);
+
+  /**
    * Puts the given long (int64_t) into the preferences table.
    *
    * The key may not have any whitespace nor an equals sign.
@@ -172,6 +202,12 @@
   void PutLong(wpi::StringRef key, int64_t value);
 
   /**
+   * Puts the given long into the preferences table if it doesn't
+   * already exist.
+   */
+  void InitLong(wpi::StringRef key, int64_t value);
+
+  /**
    * Returns whether or not there is a key with the given name.
    *
    * @param key the key
diff --git a/wpilibc/src/main/native/include/frc/RobotBase.h b/wpilibc/src/main/native/include/frc/RobotBase.h
index 725aa97..66d5637 100644
--- a/wpilibc/src/main/native/include/frc/RobotBase.h
+++ b/wpilibc/src/main/native/include/frc/RobotBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,6 +10,7 @@
 #include <chrono>
 #include <thread>
 
+#include <hal/HALBase.h>
 #include <hal/Main.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
@@ -89,6 +90,8 @@
     impl::RunRobot<Robot>(m, &robot);
   }
 
+  HAL_Shutdown();
+
   return 0;
 }
 
@@ -134,6 +137,14 @@
   bool IsAutonomous() const;
 
   /**
+   * Determine if the robot is currently in Autonomous mode and enabled.
+   *
+   * @return True if the robot us currently operating Autonomously while enabled
+   * as determined by the field controls.
+   */
+  bool IsAutonomousEnabled() const;
+
+  /**
    * Determine if the robot is currently in Operator Control mode.
    *
    * @return True if the robot is currently operating in Tele-Op mode as
@@ -142,6 +153,14 @@
   bool IsOperatorControl() const;
 
   /**
+   * Determine if the robot is current in Operator Control mode and enabled.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode while
+   * wnabled as determined by the field-controls.
+   */
+  bool IsOperatorControlEnabled() const;
+
+  /**
    * Determine if the robot is currently in Test mode.
    *
    * @return True if the robot is currently running tests as determined by the
@@ -166,6 +185,11 @@
 
   virtual void EndCompetition() = 0;
 
+  /**
+   * Get if the robot is real.
+   *
+   * @return If the robot is running in the real world.
+   */
   static constexpr bool IsReal() {
 #ifdef __FRC_ROBORIO__
     return true;
@@ -174,9 +198,13 @@
 #endif
   }
 
+  /**
+   * Get if the robot is a simulation.
+   *
+   * @return If the robot is running in simulation.
+   */
   static constexpr bool IsSimulation() { return !IsReal(); }
 
- protected:
   /**
    * Constructor for a generic robot program.
    *
@@ -192,6 +220,7 @@
 
   virtual ~RobotBase();
 
+ protected:
   // m_ds isn't moved in these because DriverStation is a singleton; every
   // instance of RobotBase has a reference to the same object.
   RobotBase(RobotBase&&) noexcept;
diff --git a/wpilibc/src/main/native/include/frc/RobotDrive.h b/wpilibc/src/main/native/include/frc/RobotDrive.h
index 3580003..a8b63e6 100644
--- a/wpilibc/src/main/native/include/frc/RobotDrive.h
+++ b/wpilibc/src/main/native/include/frc/RobotDrive.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -30,6 +30,9 @@
  * supplied on creation of the class. Those are used for either the Drive
  * function (intended for hand created drive code, such as autonomous) or with
  * the Tank/Arcade functions intended to be used for Operator Control driving.
+ *
+ * @deprecated Use DifferentialDrive or MecanumDrive classes instead.
+ *
  */
 class RobotDrive : public MotorSafety {
  public:
@@ -346,7 +349,7 @@
   /**
    * Holonomic Drive method for Mecanum wheeled robots.
    *
-   * This is an alias to MecanumDrive_Polar() for backward compatability
+   * This is an alias to MecanumDrive_Polar() for backward compatibility
    *
    * @param magnitude The speed that the robot should drive in a given
    *                  direction. [-1.0..1.0]
diff --git a/wpilibc/src/main/native/include/frc/RobotState.h b/wpilibc/src/main/native/include/frc/RobotState.h
index 60e608a..5f1c136 100644
--- a/wpilibc/src/main/native/include/frc/RobotState.h
+++ b/wpilibc/src/main/native/include/frc/RobotState.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,8 +7,6 @@
 
 #pragma once
 
-#include <memory>
-
 namespace frc {
 
 class RobotState {
diff --git a/wpilibc/src/main/native/include/frc/SPI.h b/wpilibc/src/main/native/include/frc/SPI.h
index 9bc9564..3592775 100644
--- a/wpilibc/src/main/native/include/frc/SPI.h
+++ b/wpilibc/src/main/native/include/frc/SPI.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,7 +12,7 @@
 #include <memory>
 
 #include <hal/SPITypes.h>
-#include <units/units.h>
+#include <units/time.h>
 #include <wpi/ArrayRef.h>
 #include <wpi/deprecated.h>
 
@@ -82,15 +82,21 @@
   /**
    * Configure that the data is stable on the falling edge and the data
    * changes on the rising edge.
+   *
+   * @deprecated Use SetSampleDataOnTrailingEdge() instead.
+   *
    */
-  WPI_DEPRECATED("Use SetSampleDataOnTrailingEdge in most cases.")
+  WPI_DEPRECATED("Use SetSampleDataOnTrailingEdge instead.")
   void SetSampleDataOnFalling();
 
   /**
    * Configure that the data is stable on the rising edge and the data
    * changes on the falling edge.
+   *
+   * @deprecated Use SetSampleDataOnLeadingEdge() instead.
+   *
    */
-  WPI_DEPRECATED("Use SetSampleDataOnLeadingEdge in most cases")
+  WPI_DEPRECATED("Use SetSampleDataOnLeadingEdge instead")
   void SetSampleDataOnRising();
 
   /**
@@ -116,11 +122,11 @@
   void SetChipSelectActiveLow();
 
   /**
-   * Write data to the slave device.  Blocks until there is space in the
+   * Write data to the peripheral device.  Blocks until there is space in the
    * output FIFO.
    *
    * If not running in output only mode, also saves the data received
-   * on the MISO input during the transfer into the receive FIFO.
+   * on the CIPO input during the transfer into the receive FIFO.
    */
   virtual int Write(uint8_t* data, int size);
 
@@ -166,10 +172,10 @@
   /**
    * Set the data to be transmitted by the engine.
    *
-   * Up to 23 bytes are configurable, and may be followed by up to 127 zero
+   * Up to 16 bytes are configurable, and may be followed by up to 127 zero
    * bytes.
    *
-   * @param dataToSend data to send (maximum 23 bytes)
+   * @param dataToSend data to send (maximum 16 bytes)
    * @param zeroSize number of zeros to send after the data
    */
   void SetAutoTransmitData(wpi::ArrayRef<uint8_t> dataToSend, int zeroSize);
@@ -190,6 +196,8 @@
    * InitAuto() and SetAutoTransmitData() must be called before calling this
    * function.
    *
+   * @deprecated use unit-safe StartAutoRate(units::second_t period) instead.
+   *
    * @param period period between transfers, in seconds (us resolution)
    */
   WPI_DEPRECATED("Use StartAutoRate with unit-safety instead")
@@ -253,6 +261,10 @@
    * Blocks until numToRead words have been read or timeout expires.
    * May be called with numToRead=0 to retrieve how many words are available.
    *
+   * @deprecated Use unit safe version instead.
+   *             ReadAutoReceivedData(uint32_t* buffer, int numToRead, <!--
+   * -->         units::second_t timeout)
+   *
    * @param buffer buffer where read words are stored
    * @param numToRead number of words to read
    * @param timeout timeout in seconds (ms resolution)
@@ -304,6 +316,11 @@
   /**
    * Initialize the accumulator.
    *
+   * @deprecated Use unit-safe version instead.
+   *             InitAccumulator(units::second_t period, int cmd, int <!--
+   * -->         xferSize, int validMask, int validValue, int dataShift, <!--
+   * -->         int dataSize, bool isSigned, bool bigEndian)
+   *
    * @param period    Time between reads
    * @param cmd       SPI command to send to request data
    * @param xferSize  SPI transfer size, in bytes
diff --git a/wpilibc/src/main/native/include/frc/ScopedTracer.h b/wpilibc/src/main/native/include/frc/ScopedTracer.h
new file mode 100644
index 0000000..8634c1e
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/ScopedTracer.h
@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/Tracer.h"
+
+namespace wpi {
+class raw_ostream;
+}  // namespace wpi
+
+namespace frc {
+/**
+ * A class for keeping track of how much time it takes for different
+ * parts of code to execute. This class uses RAII, meaning you simply
+ * need to create an instance at the top of the block you are timing. After the
+ * block finishes execution (i.e. when the ScopedTracer instance gets
+ * destroyed), the epoch is printed to the provided raw_ostream.
+ */
+class ScopedTracer {
+ public:
+  /**
+   * Constructs a ScopedTracer instance.
+   *
+   * @param name The name of the epoch.
+   * @param os A reference to the raw_ostream to print data to.
+   */
+  ScopedTracer(wpi::Twine name, wpi::raw_ostream& os);
+  ~ScopedTracer();
+
+  ScopedTracer(const ScopedTracer&) = delete;
+  ScopedTracer& operator=(const ScopedTracer&) = delete;
+
+ private:
+  Tracer m_tracer;
+  std::string m_name;
+  wpi::raw_ostream& m_os;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/SerialPort.h b/wpilibc/src/main/native/include/frc/SerialPort.h
index f9edb84..7c44c75 100644
--- a/wpilibc/src/main/native/include/frc/SerialPort.h
+++ b/wpilibc/src/main/native/include/frc/SerialPort.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -106,7 +106,7 @@
    * Enable termination and specify the termination character.
    *
    * Termination is currently only implemented for receive.
-   * When the the terminator is recieved, the Read() or Scanf() will return
+   * When the the terminator is received, the Read() or Scanf() will return
    * fewer bytes than requested, stopping after the terminator.
    *
    * @param terminator The character to use for termination.
@@ -169,7 +169,7 @@
    *
    * Specify the amount of data that can be stored before data
    * from the device is returned to Read or Scanf.  If you want
-   * data that is recieved to be returned immediately, set this to 1.
+   * data that is received to be returned immediately, set this to 1.
    *
    * It the buffer is not filled before the read timeout expires, all
    * data that has been received so far will be returned.
diff --git a/wpilibc/src/main/native/include/frc/SlewRateLimiter.h b/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
index 7236a30..d18fb09 100644
--- a/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
+++ b/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -11,7 +11,7 @@
 
 #include <algorithm>
 
-#include <units/units.h>
+#include <units/time.h>
 
 namespace frc {
 /**
@@ -25,11 +25,11 @@
  */
 template <class Unit>
 class SlewRateLimiter {
+ public:
   using Unit_t = units::unit_t<Unit>;
   using Rate = units::compound_unit<Unit, units::inverse<units::seconds>>;
   using Rate_t = units::unit_t<Rate>;
 
- public:
   /**
    * Creates a new SlewRateLimiter with the given rate limit and initial value.
    *
@@ -37,9 +37,9 @@
    * @param initialValue The initial value of the input.
    */
   explicit SlewRateLimiter(Rate_t rateLimit, Unit_t initialValue = Unit_t{0})
-      : m_rateLimit{rateLimit}, m_prevVal{initialValue} {
-    m_timer.Start();
-  }
+      : m_rateLimit{rateLimit},
+        m_prevVal{initialValue},
+        m_prevTime{frc2::Timer::GetFPGATimestamp()} {}
 
   /**
    * Filters the input to limit its slew rate.
@@ -49,9 +49,11 @@
    * rate.
    */
   Unit_t Calculate(Unit_t input) {
-    m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * m_timer.Get(),
-                            m_rateLimit * m_timer.Get());
-    m_timer.Reset();
+    units::second_t currentTime = frc2::Timer::GetFPGATimestamp();
+    units::second_t elapsedTime = currentTime - m_prevTime;
+    m_prevVal += std::clamp(input - m_prevVal, -m_rateLimit * elapsedTime,
+                            m_rateLimit * elapsedTime);
+    m_prevTime = currentTime;
     return m_prevVal;
   }
 
@@ -62,13 +64,13 @@
    * @param value The value to reset to.
    */
   void Reset(Unit_t value) {
-    m_timer.Reset();
     m_prevVal = value;
+    m_prevTime = frc2::Timer::GetFPGATimestamp();
   }
 
  private:
-  frc2::Timer m_timer;
   Rate_t m_rateLimit;
   Unit_t m_prevVal;
+  units::second_t m_prevTime;
 };
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Solenoid.h b/wpilibc/src/main/native/include/frc/Solenoid.h
index 86a2839..9939b41 100644
--- a/wpilibc/src/main/native/include/frc/Solenoid.h
+++ b/wpilibc/src/main/native/include/frc/Solenoid.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -62,6 +62,14 @@
   virtual bool Get() const;
 
   /**
+   * Toggle the value of the solenoid.
+   *
+   * If the solenoid is set to on, it'll be turned off. If the solenoid is set
+   * to off, it'll be turned on.
+   */
+  void Toggle();
+
+  /**
    * Check if solenoid is blacklisted.
    *
    * If a solenoid is shorted, it is added to the blacklist and
diff --git a/wpilibc/src/main/native/include/frc/SpeedController.h b/wpilibc/src/main/native/include/frc/SpeedController.h
index 2b11aee..3053c3b 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-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/voltage.h>
 
 #include "frc/PIDOutput.h"
 
diff --git a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
index 80adf1c..e62563c 100644
--- a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
+++ b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 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.                                                               */
@@ -23,7 +23,8 @@
   template <class... SpeedControllers>
   explicit SpeedControllerGroup(SpeedController& speedController,
                                 SpeedControllers&... speedControllers);
-  ~SpeedControllerGroup() override = default;
+  explicit SpeedControllerGroup(
+      std::vector<std::reference_wrapper<SpeedController>>&& speedControllers);
 
   SpeedControllerGroup(SpeedControllerGroup&&) = default;
   SpeedControllerGroup& operator=(SpeedControllerGroup&&) = default;
@@ -41,6 +42,8 @@
  private:
   bool m_isInverted = false;
   std::vector<std::reference_wrapper<SpeedController>> m_speedControllers;
+
+  void Initialize();
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
index 5848746..d9e773d 100644
--- a/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
+++ b/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 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,19 +7,17 @@
 
 #pragma once
 
-#include "frc/smartdashboard/SendableRegistry.h"
+#include <functional>
+#include <vector>
 
 namespace frc {
 
 template <class... SpeedControllers>
 SpeedControllerGroup::SpeedControllerGroup(
     SpeedController& speedController, SpeedControllers&... speedControllers)
-    : m_speedControllers{speedController, speedControllers...} {
-  for (auto& speedController : m_speedControllers)
-    SendableRegistry::GetInstance().AddChild(this, &speedController.get());
-  static int instances = 0;
-  ++instances;
-  SendableRegistry::GetInstance().Add(this, "SpeedControllerGroup", instances);
+    : m_speedControllers(std::vector<std::reference_wrapper<SpeedController>>{
+          speedController, speedControllers...}) {
+  Initialize();
 }
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h
index 112e2c9..9dbc3f2 100644
--- a/wpilibc/src/main/native/include/frc/TimedRobot.h
+++ b/wpilibc/src/main/native/include/frc/TimedRobot.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 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,18 @@
 
 #pragma once
 
+#include <functional>
+#include <vector>
+
 #include <hal/Types.h>
-#include <units/units.h>
+#include <units/math.h>
+#include <units/time.h>
 #include <wpi/deprecated.h>
+#include <wpi/priority_queue.h>
 
 #include "frc/ErrorBase.h"
 #include "frc/IterativeRobotBase.h"
+#include "frc2/Timer.h"
 
 namespace frc {
 
@@ -47,6 +53,9 @@
   /**
    * Constructor for TimedRobot.
    *
+   * @deprecated use unit safe constructor instead.
+   * TimedRobot(units::second_t period = kDefaultPeriod)
+   *
    * @param period Period in seconds.
    */
   WPI_DEPRECATED("Use constructor with unit-safety instead.")
@@ -64,16 +73,57 @@
   TimedRobot(TimedRobot&&) = default;
   TimedRobot& operator=(TimedRobot&&) = default;
 
- private:
-  hal::Handle<HAL_NotifierHandle> m_notifier;
-
-  // The absolute expiration time
-  units::second_t m_expirationTime{0};
-
   /**
-   * Update the HAL alarm time.
+   * Add a callback to run at a specific period with a starting time offset.
+   *
+   * This is scheduled on TimedRobot's Notifier, so TimedRobot and the callback
+   * run synchronously. Interactions between them are thread-safe.
+   *
+   * @param callback The callback to run.
+   * @param period   The period at which to run the callback.
+   * @param offset   The offset from the common starting time. This is useful
+   *                 for scheduling a callback in a different timeslot relative
+   *                 to TimedRobot.
    */
-  void UpdateAlarm();
+  void AddPeriodic(std::function<void()> callback, units::second_t period,
+                   units::second_t offset = 0_s);
+
+ private:
+  class Callback {
+   public:
+    std::function<void()> func;
+    units::second_t period;
+    units::second_t expirationTime;
+
+    /**
+     * Construct a callback container.
+     *
+     * @param func      The callback to run.
+     * @param startTime The common starting point for all callback scheduling.
+     * @param period    The period at which to run the callback.
+     * @param offset    The offset from the common starting time.
+     */
+    Callback(std::function<void()> func, units::second_t startTime,
+             units::second_t period, units::second_t offset)
+        : func{func},
+          period{period},
+          expirationTime{
+              startTime + offset +
+              units::math::floor((frc2::Timer::GetFPGATimestamp() - startTime) /
+                                 period) *
+                  period +
+              period} {}
+
+    bool operator>(const Callback& rhs) const {
+      return expirationTime > rhs.expirationTime;
+    }
+  };
+
+  hal::Handle<HAL_NotifierHandle> m_notifier;
+  units::second_t m_startTime;
+
+  wpi::priority_queue<Callback, std::vector<Callback>, std::greater<Callback>>
+      m_callbacks;
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Timer.h b/wpilibc/src/main/native/include/frc/Timer.h
index 99caa47..ac166c5 100644
--- a/wpilibc/src/main/native/include/frc/Timer.h
+++ b/wpilibc/src/main/native/include/frc/Timer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,11 +7,6 @@
 
 #pragma once
 
-#include <units/units.h>
-#include <wpi/deprecated.h>
-#include <wpi/mutex.h>
-
-#include "frc/Base.h"
 #include "frc2/Timer.h"
 
 namespace frc {
@@ -132,9 +127,6 @@
    */
   static double GetMatchTime();
 
-  // The time, in seconds, at which the 32-bit FPGA timestamp rolls over to 0
-  static const double kRolloverTime;
-
  private:
   frc2::Timer m_timer;
 };
diff --git a/wpilibc/src/main/native/include/frc/Tracer.h b/wpilibc/src/main/native/include/frc/Tracer.h
new file mode 100644
index 0000000..627b07b
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/Tracer.h
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <chrono>
+
+#include <hal/cpp/fpga_clock.h>
+#include <wpi/StringMap.h>
+#include <wpi/StringRef.h>
+
+namespace wpi {
+class raw_ostream;
+}  // namespace wpi
+
+namespace frc {
+/**
+ * A class for keeping track of how much time it takes for different parts of
+ * code to execute. This is done with epochs, that are added to calls to
+ * AddEpoch() and can be printed with a call to PrintEpochs().
+ *
+ * Epochs are a way to partition the time elapsed so that when overruns occur,
+ * one can determine which parts of an operation consumed the most time.
+ */
+class Tracer {
+ public:
+  /**
+   * Constructs a Tracer instance.
+   */
+  Tracer();
+
+  /**
+   * Restarts the epoch timer.
+   */
+  void ResetTimer();
+
+  /**
+   * Clears all epochs.
+   */
+  void ClearEpochs();
+
+  /**
+   * Adds time since last epoch to the list printed by PrintEpochs().
+   *
+   * Epochs are a way to partition the time elapsed so that when overruns occur,
+   * one can determine which parts of an operation consumed the most time.
+   *
+   * @param epochName The name to associate with the epoch.
+   */
+  void AddEpoch(wpi::StringRef epochName);
+
+  /**
+   * Prints list of epochs added so far and their times to the DriverStation.
+   */
+  void PrintEpochs();
+
+  /**
+   * Prints list of epochs added so far and their times to a stream.
+   *
+   * @param os output stream
+   */
+  void PrintEpochs(wpi::raw_ostream& os);
+
+ private:
+  static constexpr std::chrono::milliseconds kMinPrintPeriod{1000};
+
+  hal::fpga_clock::time_point m_startTime;
+  hal::fpga_clock::time_point m_lastEpochsPrintTime = hal::fpga_clock::epoch();
+
+  wpi::StringMap<std::chrono::nanoseconds> m_epochs;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/Ultrasonic.h b/wpilibc/src/main/native/include/frc/Ultrasonic.h
index 637c6fc..f6d301b 100644
--- a/wpilibc/src/main/native/include/frc/Ultrasonic.h
+++ b/wpilibc/src/main/native/include/frc/Ultrasonic.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -132,7 +132,7 @@
    *                 the ultrasonic sensors. This scheduling method assures that
    *                 the sensors are non-interfering because no two sensors fire
    *                 at the same time. If another scheduling algorithm is
-   *                 prefered, it can be implemented by pinging the sensors
+   *                 preferred, it can be implemented by pinging the sensors
    *                 manually and waiting for the results to come back.
    */
   static void SetAutomaticMode(bool enabling);
diff --git a/wpilibc/src/main/native/include/frc/Utility.h b/wpilibc/src/main/native/include/frc/Utility.h
index 3caaf84..a02e583 100644
--- a/wpilibc/src/main/native/include/frc/Utility.h
+++ b/wpilibc/src/main/native/include/frc/Utility.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -11,13 +11,8 @@
  * @file Contains global utility functions
  */
 
-#include <stdint.h>
-
-#include <string>
-
 #include <wpi/StringRef.h>
 #include <wpi/Twine.h>
-#include <wpi/deprecated.h>
 
 #define wpi_assert(condition) \
   wpi_assert_impl(condition, #condition, "", __FILE__, __LINE__, __FUNCTION__)
diff --git a/wpilibc/src/main/native/include/frc/WPILib.h b/wpilibc/src/main/native/include/frc/WPILib.h
index 9d62514..634d0f5 100644
--- a/wpilibc/src/main/native/include/frc/WPILib.h
+++ b/wpilibc/src/main/native/include/frc/WPILib.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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 @@
 
 // clang-format off
 #ifdef _MSC_VER
-#pragma message "warning: Including this header drastically increases compilation times and is bad style. Include only what you use instead."
+#pragma message("warning: Including this header drastically increases compilation times and is bad style. Include only what you use instead.")
 #else
 #warning "Including this header drastically increases compilation times and is bad style. Include only what you use instead."
 #endif
diff --git a/wpilibc/src/main/native/include/frc/Watchdog.h b/wpilibc/src/main/native/include/frc/Watchdog.h
index b36cf23..6fc3793 100644
--- a/wpilibc/src/main/native/include/frc/Watchdog.h
+++ b/wpilibc/src/main/native/include/frc/Watchdog.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 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,17 +7,15 @@
 
 #pragma once
 
-#include <chrono>
 #include <functional>
 #include <utility>
 
-#include <hal/cpp/fpga_clock.h>
-#include <units/units.h>
-#include <wpi/SafeThread.h>
-#include <wpi/StringMap.h>
+#include <units/time.h>
 #include <wpi/StringRef.h>
 #include <wpi/deprecated.h>
 
+#include "frc/Tracer.h"
+
 namespace frc {
 
 /**
@@ -34,6 +32,9 @@
   /**
    * Watchdog constructor.
    *
+   * @deprecated use unit-safe version instead.
+   * Watchdog(units::second_t timeout, std::function<void()> callback)
+   *
    * @param timeout  The watchdog's timeout in seconds with microsecond
    *                 resolution.
    * @param callback This function is called when the timeout expires.
@@ -63,8 +64,8 @@
 
   ~Watchdog();
 
-  Watchdog(Watchdog&&) = default;
-  Watchdog& operator=(Watchdog&&) = default;
+  Watchdog(Watchdog&& rhs);
+  Watchdog& operator=(Watchdog&& rhs);
 
   /**
    * Returns the time in seconds since the watchdog was last fed.
@@ -74,6 +75,9 @@
   /**
    * Sets the watchdog's timeout.
    *
+   * @deprecated use the unit safe version instead.
+   * SetTimeout(units::second_t timeout)
+   *
    * @param timeout The watchdog's timeout in seconds with microsecond
    *                resolution.
    */
@@ -142,26 +146,25 @@
 
  private:
   // Used for timeout print rate-limiting
-  static constexpr std::chrono::milliseconds kMinPrintPeriod{1000};
+  static constexpr units::second_t kMinPrintPeriod = 1_s;
 
-  hal::fpga_clock::time_point m_startTime;
-  std::chrono::nanoseconds m_timeout;
-  hal::fpga_clock::time_point m_expirationTime;
+  units::second_t m_startTime = 0_s;
+  units::second_t m_timeout;
+  units::second_t m_expirationTime = 0_s;
   std::function<void()> m_callback;
-  hal::fpga_clock::time_point m_lastTimeoutPrintTime = hal::fpga_clock::epoch();
-  hal::fpga_clock::time_point m_lastEpochsPrintTime = hal::fpga_clock::epoch();
+  units::second_t m_lastTimeoutPrintTime = 0_s;
 
-  wpi::StringMap<std::chrono::nanoseconds> m_epochs;
+  Tracer m_tracer;
   bool m_isExpired = false;
 
   bool m_suppressTimeoutMessage = false;
 
-  class Thread;
-  wpi::SafeThreadOwner<Thread>* m_owner;
+  class Impl;
+  Impl* m_impl;
 
-  bool operator>(const Watchdog& rhs);
+  bool operator>(const Watchdog& rhs) const;
 
-  static wpi::SafeThreadOwner<Thread>& GetThreadOwner();
+  static Impl* GetImpl();
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/XboxController.h b/wpilibc/src/main/native/include/frc/XboxController.h
index 3ca2f4b..ddf4dc2 100644
--- a/wpilibc/src/main/native/include/frc/XboxController.h
+++ b/wpilibc/src/main/native/include/frc/XboxController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2020 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.                                                               */
@@ -244,6 +244,15 @@
     kBack = 7,
     kStart = 8
   };
+
+  enum class Axis {
+    kLeftX = 0,
+    kRightX = 4,
+    kLeftY = 1,
+    kRightY = 5,
+    kLeftTrigger = 2,
+    kRightTrigger = 3
+  };
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
deleted file mode 100644
index 6f6e086..0000000
--- a/wpilibc/src/main/native/include/frc/controller/ArmFeedforward.h
+++ /dev/null
@@ -1,149 +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/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/ControllerUtil.h b/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h
new file mode 100644
index 0000000..7603fde
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <type_traits>
+
+#include <units/math.h>
+
+namespace frc {
+
+/**
+ * Returns modulus of error where error is the difference between the reference
+ * and a measurement.
+ *
+ * @param reference Reference input of a controller.
+ * @param measurement The current measurement.
+ * @param minimumInput The minimum value expected from the input.
+ * @param maximumInput The maximum value expected from the input.
+ */
+template <typename T>
+T GetModulusError(T reference, T measurement, T minimumInput, T maximumInput) {
+  T error = reference - measurement;
+  T modulus = maximumInput - minimumInput;
+
+  // Wrap error above maximum input
+  int numMax = (error + maximumInput) / modulus;
+  error -= numMax * modulus;
+
+  // Wrap error below minimum input
+  int numMin = (error + minimumInput) / modulus;
+  error -= numMin * modulus;
+
+  return error;
+}
+
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h
deleted file mode 100644
index c664fc4..0000000
--- a/wpilibc/src/main/native/include/frc/controller/ElevatorFeedforward.h
+++ /dev/null
@@ -1,131 +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/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/HolonomicDriveController.h b/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
new file mode 100644
index 0000000..916e1a9
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
@@ -0,0 +1,112 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/angle.h>
+#include <units/velocity.h>
+
+#include "frc/controller/PIDController.h"
+#include "frc/controller/ProfiledPIDController.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/trajectory/Trajectory.h"
+
+namespace frc {
+/**
+ * This holonomic drive controller can be used to follow trajectories using a
+ * holonomic drive train (i.e. swerve or mecanum). Holonomic trajectory
+ * following is a much simpler problem to solve compared to skid-steer style
+ * drivetrains because it is possible to individually control forward, sideways,
+ * and angular velocity.
+ *
+ * The holonomic drive controller takes in one PID controller for each
+ * direction, forward and sideways, and one profiled PID controller for the
+ * angular direction. Because the heading dynamics are decoupled from
+ * translations, users can specify a custom heading that the drivetrain should
+ * point toward. This heading reference is profiled for smoothness.
+ */
+class HolonomicDriveController {
+ public:
+  /**
+   * Constructs a holonomic drive controller.
+   *
+   * @param xController     A PID Controller to respond to error in the
+   * field-relative x direction.
+   * @param yController     A PID Controller to respond to error in the
+   * field-relative y direction.
+   * @param thetaController A profiled PID controller to respond to error in
+   * angle.
+   */
+  HolonomicDriveController(
+      const frc2::PIDController& xController,
+      const frc2::PIDController& yController,
+      const ProfiledPIDController<units::radian>& thetaController);
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   */
+  bool AtReference() const;
+
+  /**
+   * Sets the pose error which is considered tolerable for use with
+   * AtReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   */
+  void SetTolerance(const Pose2d& tolerance);
+
+  /**
+   * Returns the next output of the holonomic drive controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose        The current pose.
+   * @param poseRef            The desired pose.
+   * @param linearVelocityRef  The desired linear velocity.
+   * @param angleRef           The desired ending angle.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
+                          units::meters_per_second_t linearVelocityRef,
+                          const Rotation2d& angleRef);
+
+  /**
+   * Returns the next output of the holonomic drive controller.
+   *
+   * The reference pose, linear velocity, and angular velocity should come from
+   * a drivetrain trajectory.
+   *
+   * @param currentPose  The current pose.
+   * @param desiredState The desired pose, linear velocity, and angular velocity
+   *                     from a trajectory.
+   * @param angleRef     The desired ending angle.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose,
+                          const Trajectory::State& desiredState,
+                          const Rotation2d& angleRef);
+
+  /**
+   * Enables and disables the controller for troubleshooting purposes. When
+   * Calculate() is called on a disabled controller, only feedforward values
+   * are returned.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  void SetEnabled(bool enabled);
+
+ private:
+  Pose2d m_poseError;
+  Pose2d m_poseTolerance;
+  bool m_enabled = true;
+
+  frc2::PIDController m_xController;
+  frc2::PIDController m_yController;
+  ProfiledPIDController<units::radian> m_thetaController;
+};
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/PIDController.h b/wpilibc/src/main/native/include/frc/controller/PIDController.h
index 66a36fb..5f97c1e 100644
--- a/wpilibc/src/main/native/include/frc/controller/PIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/PIDController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 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 <functional>
 #include <limits>
 
-#include <units/units.h>
+#include <units/time.h>
 
 #include "frc/smartdashboard/Sendable.h"
 #include "frc/smartdashboard/SendableHelper.h"
@@ -117,7 +117,7 @@
   double GetSetpoint() const;
 
   /**
-   * Returns true if the error is within the tolerance of the error.
+   * Returns true if the error is within the tolerance of the setpoint.
    *
    * This will return false until at least one input value has been computed.
    */
@@ -141,6 +141,11 @@
   void DisableContinuousInput();
 
   /**
+   * Returns true if continuous input is enabled.
+   */
+  bool IsContinuousInputEnabled() const;
+
+  /**
    * Sets the minimum and maximum values for the integrator.
    *
    * When the cap is reached, the integrator value is added to the controller
@@ -193,16 +198,6 @@
 
   void InitSendable(frc::SendableBuilder& builder) override;
 
- protected:
-  /**
-   * Wraps error around for continuous inputs. The original error is returned if
-   * continuous mode is disabled.
-   *
-   * @param error The current error of the PID controller.
-   * @return Error for continuous inputs.
-   */
-  double GetContinuousError(double error) const;
-
  private:
   // Factor for "proportional" control
   double m_Kp;
@@ -220,15 +215,10 @@
 
   double m_minimumIntegral = -1.0;
 
-  // Maximum input - limit setpoint to this
   double m_maximumInput = 0;
 
-  // Minimum input - limit setpoint to this
   double m_minimumInput = 0;
 
-  // Input range - difference between maximum and minimum
-  double m_inputRange = 0;
-
   // Do the endpoints wrap around? eg. Absolute encoder
   bool m_continuous = false;
 
@@ -248,14 +238,6 @@
   double m_velocityTolerance = std::numeric_limits<double>::infinity();
 
   double m_setpoint = 0;
-
-  /**
-   * Sets the minimum and maximum values expected from the input.
-   *
-   * @param minimumInput The minimum value expected from the input.
-   * @param maximumInput The maximum value expected from the input.
-   */
-  void SetInputRange(double minimumInput, double maximumInput);
 };
 
 }  // namespace frc2
diff --git a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
index 079d96e..fd246cd 100644
--- a/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ b/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -12,8 +12,9 @@
 #include <functional>
 #include <limits>
 
-#include <units/units.h>
+#include <units/time.h>
 
+#include "frc/controller/ControllerUtil.h"
 #include "frc/controller/PIDController.h"
 #include "frc/smartdashboard/Sendable.h"
 #include "frc/smartdashboard/SendableBuilder.h"
@@ -33,6 +34,7 @@
 class ProfiledPIDController
     : public Sendable,
       public SendableHelper<ProfiledPIDController<Distance>> {
+ public:
   using Distance_t = units::unit_t<Distance>;
   using Velocity =
       units::compound_unit<Distance, units::inverse<units::seconds>>;
@@ -43,7 +45,6 @@
   using State = typename TrapezoidProfile<Distance>::State;
   using Constraints = typename TrapezoidProfile<Distance>::Constraints;
 
- public:
   /**
    * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
    * Kd. Users should call reset() when they first start running the controller
@@ -195,6 +196,8 @@
   void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
     m_controller.EnableContinuousInput(minimumInput.template to<double>(),
                                        maximumInput.template to<double>());
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
   }
 
   /**
@@ -251,6 +254,23 @@
    * @param measurement The current measurement of the process variable.
    */
   double Calculate(Distance_t measurement) {
+    if (m_controller.IsContinuousInputEnabled()) {
+      // Get error which is smallest distance between goal and measurement
+      auto goalMinDistance = frc::GetModulusError<Distance_t>(
+          m_goal.position, measurement, m_minimumInput, m_maximumInput);
+      auto setpointMinDistance = frc::GetModulusError<Distance_t>(
+          m_setpoint.position, measurement, m_minimumInput, m_maximumInput);
+
+      // Recompute the profile goal with the smallest error, thus giving the
+      // shortest path. The goal may be outside the input range after this
+      // operation, but that's OK because the controller will still go there and
+      // report an error of zero. In other words, the setpoint only needs to be
+      // offset from the measurement by the input range modulus; they don't need
+      // to be equal.
+      m_goal.position = goalMinDistance + measurement;
+      m_setpoint.position = setpointMinDistance + measurement;
+    }
+
     frc::TrapezoidProfile<Distance> profile{m_constraints, m_goal, m_setpoint};
     m_setpoint = profile.Calculate(GetPeriod());
     return m_controller.Calculate(measurement.template to<double>(),
@@ -324,12 +344,12 @@
 
   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(
+        "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}); });
@@ -337,6 +357,8 @@
 
  private:
   frc2::PIDController m_controller;
+  Distance_t m_minimumInput{0};
+  Distance_t m_maximumInput{0};
   typename frc::TrapezoidProfile<Distance>::State m_goal;
   typename frc::TrapezoidProfile<Distance>::State m_setpoint;
   typename frc::TrapezoidProfile<Distance>::Constraints m_constraints;
diff --git a/wpilibc/src/main/native/include/frc/controller/RamseteController.h b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
index 6ec6edc..e746bb3 100644
--- a/wpilibc/src/main/native/include/frc/controller/RamseteController.h
+++ b/wpilibc/src/main/native/include/frc/controller/RamseteController.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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,8 @@
 
 #pragma once
 
-#include <units/units.h>
+#include <units/angular_velocity.h>
+#include <units/velocity.h>
 
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
@@ -56,7 +57,7 @@
 
   /**
    * 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
+   * b and zeta of 2.0 and 0.7 have been well-tested to produce desirable
    * results.
    */
   RamseteController() : RamseteController(2.0, 0.7) {}
@@ -102,12 +103,20 @@
   ChassisSpeeds Calculate(const Pose2d& currentPose,
                           const Trajectory::State& desiredState);
 
+  /**
+   * Enables and disables the controller for troubleshooting purposes.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  void SetEnabled(bool enabled);
+
  private:
   double m_b;
   double m_zeta;
 
   Pose2d m_poseError;
   Pose2d m_poseTolerance;
+  bool m_enabled = true;
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
deleted file mode 100644
index 8f82cb9..0000000
--- a/wpilibc/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
+++ /dev/null
@@ -1,129 +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/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 c6b705a..86103de 100644
--- a/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -191,7 +191,7 @@
   void SetQuickStopAlpha(double alpha);
 
   /**
-   * Gets if the power sent to the right side of the drivetrain is multipled by
+   * Gets if the power sent to the right side of the drivetrain is multiplied by
    * -1.
    *
    * @return true if the right side is inverted
@@ -200,9 +200,10 @@
 
   /**
    * Sets if the power sent to the right side of the drivetrain should be
-   * multipled by -1.
+   * multiplied by -1.
    *
-   * @param rightSideInverted true if right side power should be multipled by -1
+   * @param rightSideInverted true if right side power should be multiplied by
+   * -1
    */
   void SetRightSideInverted(bool rightSideInverted);
 
diff --git a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
index 9ddb57e..8435b4d 100644
--- a/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
+++ b/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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.                                                               */
@@ -116,7 +116,7 @@
   void DrivePolar(double magnitude, double angle, double zRotation);
 
   /**
-   * Gets if the power sent to the right side of the drivetrain is multipled by
+   * Gets if the power sent to the right side of the drivetrain is multiplied by
    * -1.
    *
    * @return true if the right side is inverted
@@ -125,9 +125,10 @@
 
   /**
    * Sets if the power sent to the right side of the drivetrain should be
-   * multipled by -1.
+   * multiplied by -1.
    *
-   * @param rightSideInverted true if right side power should be multipled by -1
+   * @param rightSideInverted true if right side power should be multiplied by
+   * -1
    */
   void SetRightSideInverted(bool rightSideInverted);
 
diff --git a/wpilibc/src/main/native/include/frc/filters/Filter.h b/wpilibc/src/main/native/include/frc/filters/Filter.h
index b200407..fb14d28 100644
--- a/wpilibc/src/main/native/include/frc/filters/Filter.h
+++ b/wpilibc/src/main/native/include/frc/filters/Filter.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 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.                                                               */
@@ -17,6 +17,8 @@
 
 /**
  * Interface for filters
+ *
+ * @deprecated only used by the deprecated LinearDigitalFilter
  */
 class Filter : public PIDSource {
  public:
diff --git a/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h b/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
index 0a2afd2..c4fc3ef 100644
--- a/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
+++ b/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2015-2020 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.                                                               */
@@ -68,6 +68,8 @@
  * definitely need to adjust the gains if you then want to run it at 200Hz!
  * Combining this with Note 1 - the impetus is on YOU as a developer to make
  * sure PIDGet() gets called at the desired, constant frequency!
+ *
+ * @deprecated Use LinearFilter class instead
  */
 class LinearDigitalFilter : public Filter {
  public:
diff --git a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h b/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
deleted file mode 100644
index 46328a0..0000000
--- a/wpilibc/src/main/native/include/frc/geometry/Pose2d.h
+++ /dev/null
@@ -1,179 +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 "Transform2d.h"
-#include "Translation2d.h"
-#include "Twist2d.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
-
-namespace frc {
-
-/**
- * Represents a 2d pose containing translational and rotational elements.
- */
-class Pose2d {
- public:
-  /**
-   * Constructs a pose at the origin facing toward the positive X axis.
-   * (Translation2d{0, 0} and Rotation{0})
-   */
-  constexpr Pose2d() = default;
-
-  /**
-   * Constructs a pose with the specified translation and rotation.
-   *
-   * @param translation The translational component of the pose.
-   * @param rotation The rotational component of the pose.
-   */
-  Pose2d(Translation2d translation, Rotation2d rotation);
-
-  /**
-   * Convenience constructors that takes in x and y values directly instead of
-   * having to construct a Translation2d.
-   *
-   * @param x The x component of the translational component of the pose.
-   * @param y The y component of the translational component of the pose.
-   * @param rotation The rotational component of the pose.
-   */
-  Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation);
-
-  /**
-   * Transforms the pose by the given transformation and returns the new
-   * transformed pose.
-   *
-   * [x_new]    [cos, -sin, 0][transform.x]
-   * [y_new] += [sin,  cos, 0][transform.y]
-   * [t_new]    [0,    0,   1][transform.t]
-   *
-   * @param other The transform to transform the pose by.
-   *
-   * @return The transformed pose.
-   */
-  Pose2d operator+(const Transform2d& other) const;
-
-  /**
-   * Transforms the current pose by the transformation.
-   *
-   * This is similar to the + operator, except that it mutates the current
-   * object.
-   *
-   * @param other The transform to transform the pose by.
-   *
-   * @return Reference to the new mutated object.
-   */
-  Pose2d& operator+=(const Transform2d& other);
-
-  /**
-   * Returns the Transform2d that maps the one pose to another.
-   *
-   * @param other The initial pose of the transformation.
-   * @return The transform that maps the other pose to the current pose.
-   */
-  Transform2d operator-(const Pose2d& other) const;
-
-  /**
-   * Checks equality between this Pose2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
-   */
-  bool operator==(const Pose2d& other) const;
-
-  /**
-   * Checks inequality between this Pose2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Pose2d& other) const;
-
-  /**
-   * Returns the underlying translation.
-   *
-   * @return Reference to the translational component of the pose.
-   */
-  const Translation2d& Translation() const { return m_translation; }
-
-  /**
-   * Returns the underlying rotation.
-   *
-   * @return Reference to the rotational component of the pose.
-   */
-  const Rotation2d& Rotation() const { return m_rotation; }
-
-  /**
-   * Transforms the pose by the given transformation and returns the new pose.
-   * See + operator for the matrix multiplication performed.
-   *
-   * @param other The transform to transform the pose by.
-   *
-   * @return The transformed pose.
-   */
-  Pose2d TransformBy(const Transform2d& other) const;
-
-  /**
-   * Returns the other pose relative to the current pose.
-   *
-   * This function can often be used for trajectory tracking or pose
-   * stabilization algorithms to get the error between the reference and the
-   * current pose.
-   *
-   * @param other The pose that is the origin of the new coordinate frame that
-   * the current pose will be converted into.
-   *
-   * @return The current pose relative to the new origin pose.
-   */
-  Pose2d RelativeTo(const Pose2d& other) const;
-
-  /**
-   * Obtain a new Pose2d from a (constant curvature) velocity.
-   *
-   * See <https://file.tavsys.net/control/state-space-guide.pdf> section on
-   * nonlinear pose estimation for derivation.
-   *
-   * The twist is a change in pose in the robot's coordinate frame since the
-   * previous pose update. When the user runs exp() on the previous known
-   * field-relative pose with the argument being the twist, the user will
-   * receive the new field-relative pose.
-   *
-   * "Exp" represents the pose exponential, which is solving a differential
-   * equation moving the pose forward in time.
-   *
-   * @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 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.
-   */
-  Pose2d Exp(const Twist2d& twist) const;
-
-  /**
-   * Returns a Twist2d that maps this pose to the end pose. If c is the output
-   * of a.Log(b), then a.Exp(c) would yield b.
-   *
-   * @param end The end pose for the transformation.
-   *
-   * @return The twist that maps this to end.
-   */
-  Twist2d Log(const Pose2d& end) const;
-
- private:
-  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
deleted file mode 100644
index b636513..0000000
--- a/wpilibc/src/main/native/include/frc/geometry/Rotation2d.h
+++ /dev/null
@@ -1,186 +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>
-
-namespace wpi {
-class json;
-}  // namespace wpi
-
-namespace frc {
-
-/**
- * A rotation in a 2d coordinate frame represented a point on the unit circle
- * (cosine and sine).
- */
-class Rotation2d {
- public:
-  /**
-   * Constructs a Rotation2d with a default angle of 0 degrees.
-   */
-  constexpr Rotation2d() = default;
-
-  /**
-   * Constructs a Rotation2d with the given radian value.
-   *
-   * @param value The value of the angle in radians.
-   */
-  Rotation2d(units::radian_t value);  // NOLINT(runtime/explicit)
-
-  /**
-   * Constructs a Rotation2d with the given x and y (cosine and sine)
-   * components. The x and y don't have to be normalized.
-   *
-   * @param x The x component or cosine of the rotation.
-   * @param y The y component or sine of the rotation.
-   */
-  Rotation2d(double x, double y);
-
-  /**
-   * Adds two rotations together, with the result being bounded between -pi and
-   * pi.
-   *
-   * For example, Rotation2d.FromDegrees(30) + Rotation2d.FromDegrees(60) =
-   * Rotation2d{-pi/2}
-   *
-   * @param other The rotation to add.
-   *
-   * @return The sum of the two rotations.
-   */
-  Rotation2d operator+(const Rotation2d& other) const;
-
-  /**
-   * Adds a rotation to the current rotation.
-   *
-   * This is similar to the + operator except that it mutates the current
-   * object.
-   *
-   * @param other The rotation to add.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Rotation2d& operator+=(const Rotation2d& other);
-
-  /**
-   * Subtracts the new rotation from the current rotation and returns the new
-   * rotation.
-   *
-   * For example, Rotation2d.FromDegrees(10) - Rotation2d.FromDegrees(100) =
-   * Rotation2d{-pi/2}
-   *
-   * @param other The rotation to subtract.
-   *
-   * @return The difference between the two rotations.
-   */
-  Rotation2d operator-(const Rotation2d& other) const;
-
-  /**
-   * Subtracts the new rotation from the current rotation.
-   *
-   * This is similar to the - operator except that it mutates the current
-   * object.
-   *
-   * @param other The rotation to subtract.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Rotation2d& operator-=(const Rotation2d& other);
-
-  /**
-   * Takes the inverse of the current rotation. This is simply the negative of
-   * the current angular value.
-   *
-   * @return The inverse of the current rotation.
-   */
-  Rotation2d operator-() const;
-
-  /**
-   * Multiplies the current rotation by a scalar.
-   * @param scalar The scalar.
-   *
-   * @return The new scaled Rotation2d.
-   */
-  Rotation2d operator*(double scalar) const;
-
-  /**
-   * Checks equality between this Rotation2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
-   */
-  bool operator==(const Rotation2d& other) const;
-
-  /**
-   * Checks inequality between this Rotation2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Rotation2d& other) const;
-
-  /**
-   * Adds the new rotation to the current rotation using a rotation matrix.
-   *
-   * [cos_new]   [other.cos, -other.sin][cos]
-   * [sin_new] = [other.sin,  other.cos][sin]
-   *
-   * value_new = std::atan2(cos_new, sin_new)
-   *
-   * @param other The rotation to rotate by.
-   *
-   * @return The new rotated Rotation2d.
-   */
-  Rotation2d RotateBy(const Rotation2d& other) const;
-
-  /**
-   * Returns the radian value of the rotation.
-   *
-   * @return The radian value of the rotation.
-   */
-  units::radian_t Radians() const { return m_value; }
-
-  /**
-   * Returns the degree value of the rotation.
-   *
-   * @return The degree value of the rotation.
-   */
-  units::degree_t Degrees() const { return m_value; }
-
-  /**
-   * Returns the cosine of the rotation.
-   *
-   * @return The cosine of the rotation.
-   */
-  double Cos() const { return m_cos; }
-
-  /**
-   * Returns the sine of the rotation.
-   *
-   * @return The sine of the rotation.
-   */
-  double Sin() const { return m_sin; }
-
-  /**
-   * Returns the tangent of the rotation.
-   *
-   * @return The tangent of the rotation.
-   */
-  double Tan() const { return m_sin / m_cos; }
-
- private:
-  units::radian_t m_value = 0_deg;
-  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/Transform2d.h b/wpilibc/src/main/native/include/frc/geometry/Transform2d.h
deleted file mode 100644
index c75fbeb..0000000
--- a/wpilibc/src/main/native/include/frc/geometry/Transform2d.h
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "Translation2d.h"
-
-namespace frc {
-
-class Pose2d;
-
-/**
- * Represents a transformation for a Pose2d.
- */
-class Transform2d {
- public:
-  /**
-   * Constructs the transform that maps the initial pose to the final pose.
-   *
-   * @param initial The initial pose for the transformation.
-   * @param final The final pose for the transformation.
-   */
-  Transform2d(Pose2d initial, Pose2d final);
-
-  /**
-   * Constructs a transform with the given translation and rotation components.
-   *
-   * @param translation Translational component of the transform.
-   * @param rotation Rotational component of the transform.
-   */
-  Transform2d(Translation2d translation, Rotation2d rotation);
-
-  /**
-   * Constructs the identity transform -- maps an initial pose to itself.
-   */
-  constexpr Transform2d() = default;
-
-  /**
-   * Returns the translation component of the transformation.
-   *
-   * @return Reference to the translational component of the transform.
-   */
-  const Translation2d& Translation() const { return m_translation; }
-
-  /**
-   * Returns the rotational component of the transformation.
-   *
-   * @return Reference to the rotational component of the transform.
-   */
-  const Rotation2d& Rotation() const { return m_rotation; }
-
-  /**
-   * Scales the transform by the scalar.
-   *
-   * @param scalar The scalar.
-   * @return The scaled Transform2d.
-   */
-  Transform2d operator*(double scalar) const {
-    return Transform2d(m_translation * scalar, m_rotation * scalar);
-  }
-
-  /**
-   * Checks equality between this Transform2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
-   */
-  bool operator==(const Transform2d& other) const;
-
-  /**
-   * Checks inequality between this Transform2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Transform2d& other) const;
-
- private:
-  Translation2d m_translation;
-  Rotation2d m_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
deleted file mode 100644
index 90c61e2..0000000
--- a/wpilibc/src/main/native/include/frc/geometry/Translation2d.h
+++ /dev/null
@@ -1,223 +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 "Rotation2d.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
-
-namespace frc {
-
-/**
- * Represents a translation in 2d space.
- * This object can be used to represent a point or a vector.
- *
- * This assumes that you are using conventional mathematical axes.
- * When the robot is placed on the origin, facing toward the X direction,
- * moving forward increases the X, whereas moving to the left increases the Y.
- */
-class Translation2d {
- public:
-  /**
-   * Constructs a Translation2d with X and Y components equal to zero.
-   */
-  constexpr Translation2d() = default;
-
-  /**
-   * Constructs a Translation2d with the X and Y components equal to the
-   * provided values.
-   *
-   * @param x The x component of the translation.
-   * @param y The y component of the translation.
-   */
-  Translation2d(units::meter_t x, units::meter_t y);
-
-  /**
-   * Calculates the distance between two translations in 2d space.
-   *
-   * This function uses the pythagorean theorem to calculate the distance.
-   * distance = std::sqrt((x2 - x1)^2 + (y2 - y1)^2)
-   *
-   * @param other The translation to compute the distance to.
-   *
-   * @return The distance between the two translations.
-   */
-  units::meter_t Distance(const Translation2d& other) const;
-
-  /**
-   * Returns the X component of the translation.
-   *
-   * @return The x component of the translation.
-   */
-  units::meter_t X() const { return m_x; }
-
-  /**
-   * Returns the Y component of the translation.
-   *
-   * @return The y component of the translation.
-   */
-  units::meter_t Y() const { return m_y; }
-
-  /**
-   * Returns the norm, or distance from the origin to the translation.
-   *
-   * @return The norm of the translation.
-   */
-  units::meter_t Norm() const;
-
-  /**
-   * Applies a rotation to the translation in 2d space.
-   *
-   * This multiplies the translation vector by a counterclockwise rotation
-   * matrix of the given angle.
-   *
-   * [x_new]   [other.cos, -other.sin][x]
-   * [y_new] = [other.sin,  other.cos][y]
-   *
-   * For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
-   * Translation2d of {0, 2}.
-   *
-   * @param other The rotation to rotate the translation by.
-   *
-   * @return The new rotated translation.
-   */
-  Translation2d RotateBy(const Rotation2d& other) const;
-
-  /**
-   * Adds two translations in 2d space and returns the sum. This is similar to
-   * vector addition.
-   *
-   * For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
-   * Translation2d{3.0, 8.0}
-   *
-   * @param other The translation to add.
-   *
-   * @return The sum of the translations.
-   */
-  Translation2d operator+(const Translation2d& other) const;
-
-  /**
-   * Adds the new translation to the current translation.
-   *
-   * This is similar to the + operator, except that the current object is
-   * mutated.
-   *
-   * @param other The translation to add.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Translation2d& operator+=(const Translation2d& other);
-
-  /**
-   * Subtracts the other translation from the other translation and returns the
-   * difference.
-   *
-   * For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
-   * Translation2d{4.0, 2.0}
-   *
-   * @param other The translation to subtract.
-   *
-   * @return The difference between the two translations.
-   */
-  Translation2d operator-(const Translation2d& other) const;
-
-  /**
-   * Subtracts the new translation from the current translation.
-   *
-   * This is similar to the - operator, except that the current object is
-   * mutated.
-   *
-   * @param other The translation to subtract.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Translation2d& operator-=(const Translation2d& other);
-
-  /**
-   * Returns the inverse of the current translation. This is equivalent to
-   * rotating by 180 degrees, flipping the point over both axes, or simply
-   * negating both components of the translation.
-   *
-   * @return The inverse of the current translation.
-   */
-  Translation2d operator-() const;
-
-  /**
-   * Multiplies the translation by a scalar and returns the new translation.
-   *
-   * For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
-   *
-   * @param scalar The scalar to multiply by.
-   *
-   * @return The scaled translation.
-   */
-  Translation2d operator*(double scalar) const;
-
-  /**
-   * Multiplies the current translation by a scalar.
-   *
-   * This is similar to the * operator, except that current object is mutated.
-   *
-   * @param scalar The scalar to multiply by.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Translation2d& operator*=(double scalar);
-
-  /**
-   * Divides the translation by a scalar and returns the new translation.
-   *
-   * For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
-   *
-   * @param scalar The scalar to divide by.
-   *
-   * @return The scaled translation.
-   */
-  Translation2d operator/(double scalar) const;
-
-  /**
-   * Checks equality between this Translation2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
-   */
-  bool operator==(const Translation2d& other) const;
-
-  /**
-   * Checks inequality between this Translation2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Translation2d& other) const;
-
-  /*
-   * Divides the current translation by a scalar.
-   *
-   * This is similar to the / operator, except that current object is mutated.
-   *
-   * @param scalar The scalar to divide by.
-   *
-   * @return The reference to the new mutated object.
-   */
-  Translation2d& operator/=(double scalar);
-
- private:
-  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/geometry/Twist2d.h b/wpilibc/src/main/native/include/frc/geometry/Twist2d.h
deleted file mode 100644
index ab246a0..0000000
--- a/wpilibc/src/main/native/include/frc/geometry/Twist2d.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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-#include <units/units.h>
-
-namespace frc {
-/**
- * A change in distance along arc since the last pose update. We can use ideas
- * from differential calculus to create new Pose2ds from a Twist2d and vise
- * versa.
- *
- * A Twist can be used to represent a difference between two poses.
- */
-struct Twist2d {
-  /**
-   * Linear "dx" component
-   */
-  units::meter_t dx = 0_m;
-
-  /**
-   * Linear "dy" component
-   */
-  units::meter_t dy = 0_m;
-
-  /**
-   * Angular "dtheta" component (radians)
-   */
-  units::radian_t dtheta = 0_rad;
-
-  /**
-   * Checks equality between this Twist2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
-   */
-  bool operator==(const Twist2d& other) const {
-    return units::math::abs(dx - other.dx) < 1E-9_m &&
-           units::math::abs(dy - other.dy) < 1E-9_m &&
-           units::math::abs(dtheta - other.dtheta) < 1E-9_rad;
-  }
-
-  /**
-   * Checks inequality between this Twist2d and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const Twist2d& other) const { return !operator==(other); }
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/interfaces/Gyro.h b/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
index b5aa332..8c06993 100644
--- a/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
+++ b/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 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,10 @@
 
 #pragma once
 
+#include <units/angle.h>
+
+#include "frc/geometry/Rotation2d.h"
+
 namespace frc {
 
 /**
@@ -21,12 +25,10 @@
   Gyro& operator=(Gyro&&) = default;
 
   /**
-   * Calibrate the gyro by running for a number of samples and computing the
-   * center value. Then use the center value as the Accumulator center value for
-   * subsequent measurements. It's important to make sure that the robot is not
-   * moving while the centering calculations are in progress, this is typically
+   * Calibrate the gyro. It's important to make sure that the robot is not
+   * moving while the calibration is in progress, this is typically
    * done when the robot is first turned on while it's sitting at rest before
-   * the competition starts.
+   * the match starts.
    */
   virtual void Calibrate() = 0;
 
@@ -38,17 +40,14 @@
   virtual void Reset() = 0;
 
   /**
-   * Return the actual angle in degrees that the robot is currently facing.
+   * Return the heading of the robot in degrees.
    *
-   * The angle is based on the current accumulator value corrected by the
-   * oversampling rate, the gyro type and the A/D calibration values. The angle
-   * is continuous, that is it will continue from 360 to 361 degrees. This
-   * allows algorithms that wouldn't want to see a discontinuity in the gyro
-   * output as it sweeps past from 360 to 0 on the second time around.
+   * The angle is continuous, that is it will continue from 360 to 361 degrees.
+   * This allows algorithms that wouldn't want to see a discontinuity in the
+   * gyro output as it sweeps past from 360 to 0 on the second time around.
    *
    * The angle is expected to increase as the gyro turns clockwise when looked
-   * at from the top. It needs to follow NED axis conventions in order to work
-   * properly with dependent control loops.
+   * at from the top. It needs to follow the NED axis convention.
    *
    * @return the current heading of the robot in degrees. This heading is based
    *         on integration of the returned rate from the gyro.
@@ -61,12 +60,28 @@
    * The rate is based on the most recent reading of the gyro analog value.
    *
    * The rate is expected to be positive as the gyro turns clockwise when looked
-   * at from the top. It needs to follow NED axis conventions in order to work
-   * properly with dependent control loops.
+   * at from the top. It needs to follow the NED axis convention.
    *
    * @return the current rate in degrees per second
    */
   virtual double GetRate() const = 0;
+
+  /**
+   * Return the heading of the robot as a Rotation2d.
+   *
+   * The angle is continuous, that is it will continue from 360 to 361 degrees.
+   * This allows algorithms that wouldn't want to see a discontinuity in the
+   * gyro output as it sweeps past from 360 to 0 on the second time around.
+   *
+   * The angle is expected to increase as the gyro turns counterclockwise when
+   * looked at from the top. It needs to follow the NWU axis convention.
+   *
+   * @return the current heading of the robot as a Rotation2d. This heading is
+   *         based on integration of the returned rate from the gyro.
+   */
+  virtual Rotation2d GetRotation2d() const {
+    return Rotation2d{units::degree_t{-GetAngle()}};
+  }
 };
 
 }  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h
deleted file mode 100644
index 8c772c0..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/ChassisSpeeds.h
+++ /dev/null
@@ -1,65 +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/geometry/Rotation2d.h"
-
-namespace frc {
-/**
- * Represents the speed of a robot chassis. Although this struct contains
- * similar members compared to a Twist2d, they do NOT represent the same thing.
- * Whereas a Twist2d represents a change in pose w.r.t to the robot frame of
- * reference, this ChassisSpeeds struct represents a velocity w.r.t to the robot
- * frame of reference.
- *
- * A strictly non-holonomic drivetrain, such as a differential drive, should
- * never have a dy component because it can never move sideways. Holonomic
- * drivetrains such as swerve and mecanum will often have all three components.
- */
-struct ChassisSpeeds {
-  /**
-   * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
-   */
-  units::meters_per_second_t vx = 0_mps;
-
-  /**
-   * Represents strafe velocity w.r.t the robot frame of reference. (Left is +)
-   */
-  units::meters_per_second_t vy = 0_mps;
-
-  /**
-   * Represents the angular velocity of the robot frame. (CCW is +)
-   */
-  units::radians_per_second_t omega = 0_rad_per_s;
-
-  /**
-   * Converts a user provided field-relative set of speeds into a robot-relative
-   * ChassisSpeeds object.
-   *
-   * @param vx The component of speed in the x direction relative to the field.
-   * Positive x is away from your alliance wall.
-   * @param vy The component of speed in the y direction relative to the field.
-   * Positive y is to your left when standing behind the alliance wall.
-   * @param omega The angular rate of the robot.
-   * @param robotAngle The angle of the robot as measured by a gyroscope. The
-   * robot's angle is considered to be zero when it is facing directly away from
-   * your alliance station wall. Remember that this should be CCW positive.
-   *
-   * @return ChassisSpeeds object representing the speeds in the robot's frame
-   * of reference.
-   */
-  static ChassisSpeeds FromFieldRelativeSpeeds(
-      units::meters_per_second_t vx, units::meters_per_second_t vy,
-      units::radians_per_second_t omega, const Rotation2d& robotAngle) {
-    return {vx * robotAngle.Cos() + vy * robotAngle.Sin(),
-            -vx * robotAngle.Sin() + vy * robotAngle.Cos(), omega};
-  }
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
deleted file mode 100644
index e986029..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ /dev/null
@@ -1,70 +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 <hal/FRCUsageReporting.h>
-#include <units/units.h>
-
-#include "frc/kinematics/ChassisSpeeds.h"
-#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
-
-namespace frc {
-/**
- * Helper class that converts a chassis velocity (dx and dtheta components) to
- * left and right wheel velocities for a differential drive.
- *
- * Inverse kinematics converts a desired chassis speed into left and right
- * velocity components whereas forward kinematics converts left and right
- * component velocities into a linear and angular chassis speed.
- */
-class DifferentialDriveKinematics {
- public:
-  /**
-   * Constructs a differential drive kinematics object.
-   *
-   * @param trackWidth The track width of the drivetrain. Theoretically, this is
-   * the distance between the left wheels and right wheels. However, the
-   * empirical value may be larger than the physical measured value due to
-   * scrubbing effects.
-   */
-  explicit DifferentialDriveKinematics(units::meter_t trackWidth)
-      : trackWidth(trackWidth) {
-    HAL_Report(HALUsageReporting::kResourceType_Kinematics,
-               HALUsageReporting::kKinematics_DifferentialDrive);
-  }
-
-  /**
-   * Returns a chassis speed from left and right component velocities using
-   * forward kinematics.
-   *
-   * @param wheelSpeeds The left and right velocities.
-   * @return The chassis speed.
-   */
-  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
-   * inverse kinematics.
-   *
-   * @param chassisSpeeds The linear and angular (dx and dtheta) components that
-   * represent the chassis' speed.
-   * @return The left and right velocities.
-   */
-  constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
-      const ChassisSpeeds& chassisSpeeds) const {
-    return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
-            chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
-  }
-
-  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
deleted file mode 100644
index 379839d..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ /dev/null
@@ -1,89 +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 "DifferentialDriveKinematics.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc2/Timer.h"
-
-namespace frc {
-/**
- * Class for differential drive odometry. Odometry allows you to track the
- * robot's position on the field over the course of a match using readings from
- * 2 encoders and a gyroscope.
- *
- * Teams can use odometry during the autonomous period for complex tasks like
- * path following. Furthermore, odometry can be used for latency compensation
- * when using computer-vision systems.
- *
- * 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 gyroAngle The angle reported by the gyroscope.
-   * @param initialPose The starting position of the robot on the field.
-   */
-  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, const Rotation2d& gyroAngle) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
-    m_prevLeftDistance = 0_m;
-    m_prevRightDistance = 0_m;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
-
-  /**
-   * 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 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& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
-                       units::meter_t rightDistance);
-
- private:
-  Pose2d m_pose;
-
-  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/DifferentialDriveWheelSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
deleted file mode 100644
index 66bd84e..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
+++ /dev/null
@@ -1,39 +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>
-
-namespace frc {
-/**
- * Represents the wheel speeds for a differential drive drivetrain.
- */
-struct DifferentialDriveWheelSpeeds {
-  /**
-   * Speed of the left side of the robot.
-   */
-  units::meters_per_second_t left = 0_mps;
-
-  /**
-   * Speed of the right side of the robot.
-   */
-  units::meters_per_second_t right = 0_mps;
-
-  /**
-   * 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
-   * this issue, one can "normalize" all the wheel speeds to make sure that all
-   * requested module speeds are below the absolute threshold, while maintaining
-   * the ratio of speeds between modules.
-   *
-   * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
-   */
-  void Normalize(units::meters_per_second_t attainableMaxSpeed);
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
deleted file mode 100644
index 47b1b34..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ /dev/null
@@ -1,144 +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 <Eigen/Core>
-#include <Eigen/QR>
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/geometry/Translation2d.h"
-#include "frc/kinematics/ChassisSpeeds.h"
-#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
-
-namespace frc {
-
-/**
- * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
- * into individual wheel speeds.
- *
- * The inverse kinematics (converting from a desired chassis velocity to
- * individual wheel speeds) uses the relative locations of the wheels with
- * respect to the center of rotation. The center of rotation for inverse
- * kinematics is also variable. This means that you can set your set your center
- * of rotation in a corner of the robot to perform special evasion manuevers.
- *
- * Forward kinematics (converting an array of wheel speeds into the overall
- * chassis motion) is performs the exact opposite of what inverse kinematics
- * does. Since this is an overdetermined system (more equations than variables),
- * we use a least-squares approximation.
- *
- * The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
- * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
- * multiply by [wheelSpeeds] to get our chassis speeds.
- *
- * Forward kinematics is also used for odometry -- determining the position of
- * the robot on the field using encoders and a gyro.
- */
-class MecanumDriveKinematics {
- public:
-  /**
-   * Constructs a mecanum drive kinematics object.
-   *
-   * @param frontLeftWheel The location of the front-left wheel relative to the
-   *                       physical center of the robot.
-   * @param frontRightWheel The location of the front-right wheel relative to
-   *                        the physical center of the robot.
-   * @param rearLeftWheel The location of the rear-left wheel relative to the
-   *                      physical center of the robot.
-   * @param rearRightWheel The location of the rear-right wheel relative to the
-   *                       physical center of the robot.
-   */
-  explicit MecanumDriveKinematics(Translation2d frontLeftWheel,
-                                  Translation2d frontRightWheel,
-                                  Translation2d rearLeftWheel,
-                                  Translation2d rearRightWheel)
-      : m_frontLeftWheel{frontLeftWheel},
-        m_frontRightWheel{frontRightWheel},
-        m_rearLeftWheel{rearLeftWheel},
-        m_rearRightWheel{rearRightWheel} {
-    SetInverseKinematics(frontLeftWheel, frontRightWheel, rearLeftWheel,
-                         rearRightWheel);
-    m_forwardKinematics = m_inverseKinematics.householderQr();
-    HAL_Report(HALUsageReporting::kResourceType_Kinematics,
-               HALUsageReporting::kKinematics_MecanumDrive);
-  }
-
-  MecanumDriveKinematics(const MecanumDriveKinematics&) = default;
-
-  /**
-   * Performs inverse kinematics to return the wheel speeds from a desired
-   * chassis velocity. This method is often used to convert joystick values into
-   * wheel speeds.
-   *
-   * This function also supports variable centers of rotation. During normal
-   * operations, the center of rotation is usually the same as the physical
-   * center of the robot; therefore, the argument is defaulted to that use case.
-   * However, if you wish to change the center of rotation for evasive
-   * manuevers, vision alignment, or for any other use case, you can do so.
-   *
-   * @param chassisSpeeds The desired chassis speed.
-   * @param centerOfRotation The center of rotation. For example, if you set the
-   *                         center of rotation at one corner of the robot and
-   *                         provide a chassis speed that only has a dtheta
-   *                         component, the robot will rotate around that
-   *                         corner.
-   *
-   * @return The wheel speeds. Use caution because they are not normalized.
-   *         Sometimes, a user input may cause one of the wheel speeds to go
-   *         above the attainable max velocity. Use the
-   *         MecanumDriveWheelSpeeds::Normalize() function to rectify this
-   *         issue. In addition, you can leverage the power of C++17 to directly
-   *         assign the wheel speeds to variables:
-   *
-   * @code{.cpp}
-   * auto [fl, fr, bl, br] = kinematics.ToWheelSpeeds(chassisSpeeds);
-   * @endcode
-   */
-  MecanumDriveWheelSpeeds ToWheelSpeeds(
-      const ChassisSpeeds& chassisSpeeds,
-      const Translation2d& centerOfRotation = Translation2d());
-
-  /**
-   * Performs forward kinematics to return the resulting chassis state from the
-   * given wheel speeds. This method is often used for odometry -- determining
-   * the robot's position on the field using data from the real-world speed of
-   * each wheel on the robot.
-   *
-   * @param wheelSpeeds The current mecanum drive wheel speeds.
-   *
-   * @return The resulting chassis speed.
-   */
-  ChassisSpeeds ToChassisSpeeds(const MecanumDriveWheelSpeeds& wheelSpeeds);
-
- private:
-  Eigen::Matrix<double, 4, 3> m_inverseKinematics;
-  Eigen::HouseholderQR<Eigen::Matrix<double, 4, 3>> m_forwardKinematics;
-  Translation2d m_frontLeftWheel;
-  Translation2d m_frontRightWheel;
-  Translation2d m_rearLeftWheel;
-  Translation2d m_rearRightWheel;
-
-  Translation2d m_previousCoR;
-
-  /**
-   * Construct inverse kinematics matrix from wheel locations.
-   *
-   * @param fl The location of the front-left wheel relative to the physical
-   *           center of the robot.
-   * @param fr The location of the front-right wheel relative to the physical
-   *           center of the robot.
-   * @param rl The location of the rear-left wheel relative to the physical
-   *           center of the robot.
-   * @param rr The location of the rear-right wheel relative to the physical
-   *           center of the robot.
-   */
-  void SetInverseKinematics(Translation2d fl, Translation2d fr,
-                            Translation2d rl, Translation2d rr);
-};
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
deleted file mode 100644
index 697a6b0..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/units.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/kinematics/MecanumDriveKinematics.h"
-#include "frc/kinematics/MecanumDriveWheelSpeeds.h"
-#include "frc2/Timer.h"
-
-namespace frc {
-
-/**
- * Class for mecanum drive odometry. Odometry allows you to track the robot's
- * position on the field over a course of a match using readings from your
- * mecanum wheel encoders.
- *
- * Teams can use odometry during the autonomous period for complex tasks like
- * path following. Furthermore, odometry can be used for latency compensation
- * when using computer-vision systems.
- */
-class MecanumDriveOdometry {
- public:
-  /**
-   * 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, const Rotation2d& gyroAngle) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  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.
-   *
-   * @param currentTime The current time.
-   * @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& gyroAngle,
-                               MecanumDriveWheelSpeeds 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 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& gyroAngle,
-                       MecanumDriveWheelSpeeds wheelSpeeds) {
-    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
-                          wheelSpeeds);
-  }
-
- private:
-  MecanumDriveKinematics m_kinematics;
-  Pose2d m_pose;
-
-  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/MecanumDriveWheelSpeeds.h b/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
deleted file mode 100644
index 159f7f0..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
+++ /dev/null
@@ -1,49 +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>
-
-namespace frc {
-/**
- * Represents the wheel speeds for a mecanum drive drivetrain.
- */
-struct MecanumDriveWheelSpeeds {
-  /**
-   * Speed of the front-left wheel.
-   */
-  units::meters_per_second_t frontLeft = 0_mps;
-
-  /**
-   * Speed of the front-right wheel.
-   */
-  units::meters_per_second_t frontRight = 0_mps;
-
-  /**
-   * Speed of the rear-left wheel.
-   */
-  units::meters_per_second_t rearLeft = 0_mps;
-
-  /**
-   * Speed of the rear-right wheel.
-   */
-  units::meters_per_second_t rearRight = 0_mps;
-
-  /**
-   * 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
-   * this issue, one can "normalize" all the wheel speeds to make sure that all
-   * requested module speeds are below the absolute threshold, while maintaining
-   * the ratio of speeds between modules.
-   *
-   * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
-   */
-  void Normalize(units::meters_per_second_t attainableMaxSpeed);
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
deleted file mode 100644
index f889363..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ /dev/null
@@ -1,170 +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 <array>
-#include <cstddef>
-
-#include <Eigen/Core>
-#include <Eigen/QR>
-#include <hal/FRCUsageReporting.h>
-#include <units/units.h>
-
-#include "frc/geometry/Rotation2d.h"
-#include "frc/geometry/Translation2d.h"
-#include "frc/kinematics/ChassisSpeeds.h"
-#include "frc/kinematics/SwerveModuleState.h"
-
-namespace frc {
-/**
- * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
- * into individual module states (speed and angle).
- *
- * The inverse kinematics (converting from a desired chassis velocity to
- * individual module states) uses the relative locations of the modules with
- * respect to the center of rotation. The center of rotation for inverse
- * kinematics is also variable. This means that you can set your set your center
- * of rotation in a corner of the robot to perform special evasion manuevers.
- *
- * Forward kinematics (converting an array of module states into the overall
- * chassis motion) is performs the exact opposite of what inverse kinematics
- * does. Since this is an overdetermined system (more equations than variables),
- * we use a least-squares approximation.
- *
- * The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
- * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
- * multiply by [moduleStates] to get our chassis speeds.
- *
- * Forward kinematics is also used for odometry -- determining the position of
- * the robot on the field using encoders and a gyro.
- */
-template <size_t NumModules>
-class SwerveDriveKinematics {
- public:
-  /**
-   * Constructs a swerve drive kinematics object. This takes in a variable
-   * number of wheel locations as Translation2ds. The order in which you pass in
-   * the wheel locations is the same order that you will recieve the module
-   * states when performing inverse kinematics. It is also expected that you
-   * pass in the module states in the same order when calling the forward
-   * kinematics methods.
-   *
-   * @param wheels The locations of the wheels relative to the physical center
-   * of the robot.
-   */
-  template <typename... Wheels>
-  explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
-      : m_modules{wheel, wheels...} {
-    static_assert(sizeof...(wheels) >= 1,
-                  "A swerve drive requires at least two modules");
-
-    for (size_t i = 0; i < NumModules; i++) {
-      // clang-format off
-      m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
-        1, 0, (-m_modules[i].Y()).template to<double>(),
-        0, 1, (+m_modules[i].X()).template to<double>();
-      // clang-format on
-    }
-
-    m_forwardKinematics = m_inverseKinematics.householderQr();
-
-    HAL_Report(HALUsageReporting::kResourceType_Kinematics,
-               HALUsageReporting::kKinematics_SwerveDrive);
-  }
-
-  SwerveDriveKinematics(const SwerveDriveKinematics&) = default;
-
-  /**
-   * Performs inverse kinematics to return the module states from a desired
-   * chassis velocity. This method is often used to convert joystick values into
-   * module speeds and angles.
-   *
-   * This function also supports variable centers of rotation. During normal
-   * operations, the center of rotation is usually the same as the physical
-   * center of the robot; therefore, the argument is defaulted to that use case.
-   * However, if you wish to change the center of rotation for evasive
-   * manuevers, vision alignment, or for any other use case, you can do so.
-   *
-   * @param chassisSpeeds The desired chassis speed.
-   * @param centerOfRotation The center of rotation. For example, if you set the
-   * center of rotation at one corner of the robot and provide a chassis speed
-   * that only has a dtheta component, the robot will rotate around that corner.
-   *
-   * @return An array containing the module states. Use caution because these
-   * module states are not normalized. Sometimes, a user input may cause one of
-   * the module speeds to go above the attainable max velocity. Use the
-   * <NormalizeWheelSpeeds> function to rectify this issue. In addition, you can
-   * leverage the power of C++17 to directly assign the module states to
-   * variables:
-   *
-   * @code{.cpp}
-   * auto [fl, fr, bl, br] = kinematics.ToSwerveModuleStates(chassisSpeeds);
-   * @endcode
-   */
-  std::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
-      const ChassisSpeeds& chassisSpeeds,
-      const Translation2d& centerOfRotation = Translation2d());
-
-  /**
-   * 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 wheelStates The state of the modules (as a SwerveModuleState type)
-   * 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.
-   */
-  template <typename... ModuleStates>
-  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
-   * this issue, one can "normalize" all the wheel speeds to make sure that all
-   * requested module speeds are below the absolute threshold, while maintaining
-   * the ratio of speeds between modules.
-   *
-   * @param moduleStates Reference to array of module states. The array will be
-   * mutated with the normalized speeds!
-   * @param attainableMaxSpeed The absolute max speed that a module can reach.
-   */
-  static void NormalizeWheelSpeeds(
-      std::array<SwerveModuleState, NumModules>* moduleStates,
-      units::meters_per_second_t attainableMaxSpeed);
-
- private:
-  Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
-  Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
-      m_forwardKinematics;
-  std::array<Translation2d, NumModules> m_modules;
-
-  Translation2d m_previousCoR;
-};
-}  // namespace frc
-
-#include "SwerveDriveKinematics.inc"
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
deleted file mode 100644
index b362aa0..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ /dev/null
@@ -1,110 +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 <algorithm>
-
-namespace frc {
-
-template <class... Wheels>
-SwerveDriveKinematics(Translation2d, Wheels...)
-    ->SwerveDriveKinematics<1 + sizeof...(Wheels)>;
-
-template <size_t NumModules>
-std::array<SwerveModuleState, NumModules>
-SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
-    const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) {
-  // We have a new center of rotation. We need to compute the matrix again.
-  if (centerOfRotation != m_previousCoR) {
-    for (size_t i = 0; i < NumModules; i++) {
-      // clang-format off
-      m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
-        1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).template to<double>(),
-        0, 1, (+m_modules[i].X() - centerOfRotation.X()).template to<double>();
-      // clang-format on
-    }
-    m_previousCoR = centerOfRotation;
-  }
-
-  Eigen::Vector3d chassisSpeedsVector;
-  chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
-      chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
-
-  Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
-      m_inverseKinematics * chassisSpeedsVector;
-  std::array<SwerveModuleState, NumModules> moduleStates;
-
-  for (size_t i = 0; i < NumModules; i++) {
-    units::meters_per_second_t x =
-        units::meters_per_second_t{moduleStatesMatrix(i * 2, 0)};
-    units::meters_per_second_t y =
-        units::meters_per_second_t{moduleStatesMatrix(i * 2 + 1, 0)};
-
-    auto speed = units::math::hypot(x, y);
-    Rotation2d rotation{x.to<double>(), y.to<double>()};
-
-    moduleStates[i] = {speed, rotation};
-  }
-
-  return moduleStates;
-}
-
-template <size_t NumModules>
-template <typename... ModuleStates>
-ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
-    ModuleStates&&... wheelStates) {
-  static_assert(sizeof...(wheelStates) == NumModules,
-                "Number of modules is not consistent with number of wheel "
-                "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++) {
-    SwerveModuleState module = moduleStates[i];
-    moduleStatesMatrix.row(i * 2)
-        << module.speed.to<double>() * module.angle.Cos();
-    moduleStatesMatrix.row(i * 2 + 1)
-        << module.speed.to<double>() * module.angle.Sin();
-  }
-
-  Eigen::Vector3d chassisSpeedsVector =
-      m_forwardKinematics.solve(moduleStatesMatrix);
-
-  return {units::meters_per_second_t{chassisSpeedsVector(0)},
-          units::meters_per_second_t{chassisSpeedsVector(1)},
-          units::radians_per_second_t{chassisSpeedsVector(2)}};
-}
-
-template <size_t NumModules>
-void SwerveDriveKinematics<NumModules>::NormalizeWheelSpeeds(
-    std::array<SwerveModuleState, NumModules>* moduleStates,
-    units::meters_per_second_t attainableMaxSpeed) {
-  auto& states = *moduleStates;
-  auto realMaxSpeed = std::max_element(states.begin(), states.end(),
-                                       [](const auto& a, const auto& b) {
-                                         return units::math::abs(a.speed) <
-                                                units::math::abs(b.speed);
-                                       })
-                          ->speed;
-
-  if (realMaxSpeed > attainableMaxSpeed) {
-    for (auto& module : states) {
-      module.speed = module.speed / realMaxSpeed * attainableMaxSpeed;
-    }
-  }
-}
-
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
deleted file mode 100644
index 8093ca5..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ /dev/null
@@ -1,121 +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 <chrono>
-#include <cstddef>
-#include <ctime>
-
-#include <units/units.h>
-
-#include "SwerveDriveKinematics.h"
-#include "SwerveModuleState.h"
-#include "frc/geometry/Pose2d.h"
-#include "frc2/Timer.h"
-
-namespace frc {
-
-/**
- * Class for swerve drive odometry. Odometry allows you to track the robot's
- * position on the field over a course of a match using readings from your
- * swerve drive encoders and swerve azimuth encoders.
- *
- * Teams can use odometry during the autonomous period for complex tasks like
- * path following. Furthermore, odometry can be used for latency compensation
- * when using computer-vision systems.
- */
-template <size_t NumModules>
-class SwerveDriveOdometry {
- public:
-  /**
-   * 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, const Rotation2d& gyroAngle) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  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.
-   *
-   * @param currentTime The current time.
-   * @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.
-   *
-   * @return The new pose of the robot.
-   */
-  template <typename... ModuleStates>
-  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& gyroAngle,
-                               ModuleStates&&... moduleStates);
-
-  /**
-   * 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 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.
-   *
-   * @return The new pose of the robot.
-   */
-  template <typename... ModuleStates>
-  const Pose2d& Update(const Rotation2d& gyroAngle,
-                       ModuleStates&&... moduleStates) {
-    return UpdateWithTime(frc2::Timer::GetFPGATimestamp(), gyroAngle,
-                          moduleStates...);
-  }
-
- private:
-  SwerveDriveKinematics<NumModules> m_kinematics;
-  Pose2d m_pose;
-
-  units::second_t m_previousTime = -1_s;
-  Rotation2d m_previousAngle;
-  Rotation2d m_gyroOffset;
-};
-
-}  // namespace frc
-
-#include "SwerveDriveOdometry.inc"
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
deleted file mode 100644
index e794388..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ /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 <hal/FRCUsageReporting.h>
-
-namespace frc {
-template <size_t NumModules>
-SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
-    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;
-  HAL_Report(HALUsageReporting::kResourceType_Odometry,
-             HALUsageReporting::kOdometry_SwerveDrive);
-}
-
-template <size_t NumModules>
-template <typename... ModuleStates>
-const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
-    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);
-
-  auto newPose = m_pose.Exp(
-      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.h
deleted file mode 100644
index fbfe0d1..0000000
--- a/wpilibc/src/main/native/include/frc/kinematics/SwerveModuleState.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 <units/units.h>
-
-#include "frc/geometry/Rotation2d.h"
-
-namespace frc {
-/**
- * Represents the state of one swerve module.
- */
-struct SwerveModuleState {
-  /**
-   * Speed of the wheel of the module.
-   */
-  units::meters_per_second_t speed = 0_mps;
-
-  /**
-   * Angle of the module.
-   */
-  Rotation2d angle;
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h b/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
index c7e0ec2..b755f46 100644
--- a/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
+++ b/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2012-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2012-2020 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,7 @@
 
 #pragma once
 
+#include <functional>
 #include <memory>
 
 namespace frc {
@@ -22,6 +23,9 @@
   LiveWindow(const LiveWindow&) = delete;
   LiveWindow& operator=(const LiveWindow&) = delete;
 
+  std::function<void()> enabled;
+  std::function<void()> disabled;
+
   /**
    * Get an instance of the LiveWindow main class.
    *
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h b/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
index 8e666b7..df943b2 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 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.                                                               */
@@ -144,7 +144,7 @@
    * <br>Supported types:
    * <ul>
    * <li>Number</li>
-   * <li>{@link edu.wpi.first.wpilibj.AnalogInput}</li>
+   * <li>AnalogInput</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -163,9 +163,8 @@
    */
   kVoltageView,
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.PowerDistributionPanel
-   * PowerDistributionPanel}. <br>Supported types: <ul> <li>{@link
-   * edu.wpi.first.wpilibj.PowerDistributionPanel}</li>
+   * Displays a PowerDistributionPanel. <br>Supported types: <ul> <li>
+   * PowerDistributionPanel</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -176,49 +175,49 @@
    */
   kPowerDistributionPanel,
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser
-   * SendableChooser} with a dropdown combo box with a list of options.
+   * Displays a SendableChooser with a dropdown combo box with a list of
+   * options.
    * <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}</li>
+   * <li>SendableChooser</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
   kComboBoxChooser,
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser
-   * SendableChooser} with a toggle button for each available option.
+   * Displays a SendableChooserwith a toggle button for each available option.
    * <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}</li>
+   * <li>SendableChooser</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
   kSplitButtonChooser,
   /**
-   * Displays an {@link edu.wpi.first.wpilibj.Encoder} displaying its speed,
-   * total travelled distance, and its distance per tick. <br>Supported types:
+   * Displays an Encoder displaying its speed,
+   * total traveled distance, and its distance per tick. <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.Encoder}</li>
+   * <li>Encoder</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
   kEncoder,
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.SpeedController SpeedController}.
+   * Displays a SpeedController.
    * The speed controller will be controllable from the dashboard when test mode
    * is enabled, but will otherwise be view-only. <br>Supported types: <ul>
-   * <li>{@link edu.wpi.first.wpilibj.PWMSpeedController}</li>
-   * <li>{@link edu.wpi.first.wpilibj.DMC60}</li>
-   * <li>{@link edu.wpi.first.wpilibj.Jaguar}</li>
-   * <li>{@link edu.wpi.first.wpilibj.PWMTalonSRX}</li>
-   * <li>{@link edu.wpi.first.wpilibj.PWMVictorSPX}</li>
-   * <li>{@link edu.wpi.first.wpilibj.SD540}</li>
-   * <li>{@link edu.wpi.first.wpilibj.Spark}</li>
-   * <li>{@link edu.wpi.first.wpilibj.Talon}</li>
-   * <li>{@link edu.wpi.first.wpilibj.Victor}</li>
-   * <li>{@link edu.wpi.first.wpilibj.VictorSP}</li>
-   * <li>{@link edu.wpi.first.wpilibj.SpeedControllerGroup}</li>
+   * <li>PWMSpeedController</li>
+   * <li>DMC60</li>
+   * <li>Jaguar</li>
+   * <li>PWMTalonSRX</li>
+   * <li>PWMVictorSPX</li>
+   * <li>SD540</li>
+   * <li>Spark</li>
+   * <li>Talon</li>
+   * <li>Victor</li>
+   * <li>VictorSP</li>
+   * <li>SpeedControllerGroup</li>
+   * <li>Any custom subclass of {@code SpeedContorller}</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -231,10 +230,8 @@
   /**
    * Displays a command with a toggle button. Pressing the button will start the
    * command, and the button will automatically release when the command
-   * completes. <br>Supported types: <ul> <li>{@link
-   * edu.wpi.first.wpilibj.command.Command}</li> <li>{@link
-   * edu.wpi.first.wpilibj.command.CommandGroup}</li> <li>Any custom subclass of
-   * {@code Command} or {@code CommandGroup}</li>
+   * completes. <br>Supported types: <ul> <li>Command</li> <li>CommandGroup</li>
+   * <li>Any custom subclass of {@code Command} or {@code CommandGroup}</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
@@ -243,7 +240,7 @@
    * Displays a PID command with a checkbox and an editor for the PIDF
    * constants. Selecting the checkbox will start the command, and the checkbox
    * will automatically deselect when the command completes. <br>Supported
-   * types: <ul> <li>{@link edu.wpi.first.wpilibj.command.PIDCommand}</li>
+   * types: <ul> <li>PIDCommand</li>
    * <li>Any custom subclass of {@code PIDCommand}</li>
    * </ul>
    * <br>This widget has no custom properties.
@@ -252,7 +249,7 @@
   /**
    * Displays a PID controller with an editor for the PIDF constants and a
    * toggle switch for enabling and disabling the controller. <br>Supported
-   * types: <ul> <li>{@link edu.wpi.first.wpilibj.PIDController}</li>
+   * types: <ul> <li>PIDController</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
@@ -260,7 +257,7 @@
   /**
    * Displays an accelerometer with a number bar displaying the magnitude of the
    * acceleration and text displaying the exact value. <br>Supported types: <ul>
-   * <li>{@link edu.wpi.first.wpilibj.AnalogAccelerometer}</li>
+   * <li>AnalogAccelerometer</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -280,10 +277,8 @@
   kAccelerometer,
   /**
    * Displays a 3-axis accelerometer with a number bar for each axis'
-   * accleration. <br>Supported types: <ul> <li>{@link
-   * edu.wpi.first.wpilibj.ADXL345_I2C}</li> <li>{@link
-   * edu.wpi.first.wpilibj.ADXL345_SPI}</li> <li>{@link
-   * edu.wpi.first.wpilibj.ADXL362}</li>
+   * acceleration. <br>Supported types: <ul> <li>ADXL345_I2C</li> <li>
+   * ADXL345_SPI</li> <li>ADXL362</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -302,8 +297,8 @@
    * Displays a gyro with a dial from 0 to 360 degrees.
    * <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.ADXRS450_Gyro}</li>
-   * <li>{@link edu.wpi.first.wpilibj.AnalogGyro}</li>
+   * <li>ADXRS450_Gyro</li>
+   * <li>AnalogGyro</li>
    * <li>Any custom subclass of {@code GyroBase} (such as a MXP gyro)</li>
    * </ul>
    * <br>Custom properties:
@@ -319,8 +314,7 @@
   kGyro,
   /**
    * Displays a relay with toggle buttons for each supported mode (off, on,
-   * forward, reverse). <br>Supported types: <ul> <li>{@link
-   * edu.wpi.first.wpilibj.Relay}</li>
+   * forward, reverse). <br>Supported types: <ul> <li>Relay</li>
    * </ul>
    * <br>This widget has no custom properties.
    */
@@ -331,7 +325,7 @@
    * drivebase. The widget will be controllable if the robot is in test mode.
    * <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.drive.DifferentialDrive}</li>
+   * <li>DifferentialDrive</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -348,7 +342,7 @@
    * Displays a mecanum drive with a widget that displays the speed of each
    * wheel, and vectors for the direction and rotation of the drivebase. The
    * widget will be controllable if the robot is in test mode. <br>Supported
-   * types: <ul> <li>{@link edu.wpi.first.wpilibj.drive.MecanumDrive}</li>
+   * types: <ul> <li>MecanumDrive</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -361,8 +355,7 @@
    * Displays a camera stream.
    * <br>Supported types:
    * <ul>
-   * <li>{@link edu.wpi.cscore.VideoSource} (as long as it is streaming on an
-   * MJPEG server)</li>
+   * <li>VideoSource (as long as it is streaming on an MJPEG server)</li>
    * </ul>
    * <br>Custom properties:
    * <table>
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
index ccbc802..57e4743 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 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 <wpi/StringRef.h>
+
 namespace frc {
 
 // Maintainer note: this enum is mirrored in WPILibJ and in Shuffleboard
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h
index b202160..2be9859 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2018-2020 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.                                                               */
@@ -20,6 +20,9 @@
   explicit ShuffleboardInstance(nt::NetworkTableInstance ntInstance);
   virtual ~ShuffleboardInstance();
 
+  ShuffleboardInstance(ShuffleboardInstance&&) = default;
+  ShuffleboardInstance& operator=(ShuffleboardInstance&&) = default;
+
   frc::ShuffleboardTab& GetTab(wpi::StringRef title) override;
 
   void Update() override;
diff --git a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
index ea92b93..729da40 100644
--- a/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
+++ b/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 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.                                                               */
@@ -62,7 +62,7 @@
    * Sets the type of widget used to display the data. If not set, the default
    * widget type will be used. This method should only be used to use a widget
    * that does not come built into Shuffleboard (i.e. one that comes with a
-   * custom or thrid-party plugin). To use a widget that is built into
+   * custom or third-party plugin). To use a widget that is built into
    * Shuffleboard, use {@link #withWidget(WidgetType)} and {@link
    * BuiltInWidgets}.
    *
diff --git a/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h b/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h
new file mode 100644
index 0000000..1d2d2c3
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h
@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/SimDevice.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+class ADXRS450_Gyro;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADXRS450 gyroscope.
+ */
+class ADXRS450_GyroSim {
+ public:
+  /**
+   * Constructs from a ADXRS450_Gyro object.
+   *
+   * @param gyro ADXRS450_Gyro to simulate
+   */
+  explicit ADXRS450_GyroSim(const ADXRS450_Gyro& gyro);
+
+  /**
+   * Sets the angle.
+   *
+   * @param angle The angle (clockwise positive).
+   */
+  void SetAngle(units::degree_t angle);
+
+  /**
+   * Sets the angular rate (clockwise positive).
+   *
+   * @param rate The angular rate.
+   */
+  void SetRate(units::degrees_per_second_t rate);
+
+ private:
+  hal::SimDouble m_simAngle;
+  hal::SimDouble m_simRate;
+};
+
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/AddressableLEDSim.h b/wpilibc/src/main/native/include/frc/simulation/AddressableLEDSim.h
new file mode 100644
index 0000000..0c578f4
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AddressableLEDSim.h
@@ -0,0 +1,99 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+struct HAL_AddressableLEDData;
+
+namespace frc {
+
+class AddressableLED;
+
+namespace sim {
+
+/**
+ * Class to control a simulated addressable LED.
+ */
+class AddressableLEDSim {
+ public:
+  /**
+   * Constructs for the first addressable LED.
+   */
+  AddressableLEDSim();
+
+  /**
+   * Constructs from an AddressableLED object.
+   *
+   * @param addressableLED AddressableLED to simulate
+   */
+  explicit AddressableLEDSim(const AddressableLED& addressableLED);
+
+  /**
+   * Creates an AddressableLEDSim for a PWM channel.
+   *
+   * @param pwmChannel PWM channel
+   * @return Simulated object
+   * @throws std::out_of_range if no AddressableLED is configured for that
+   *         channel
+   */
+  static AddressableLEDSim CreateForChannel(int pwmChannel);
+
+  /**
+   * Creates an AddressableLEDSim for a simulated index.
+   * The index is incremented for each simulated AddressableLED.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static AddressableLEDSim CreateForIndex(int index);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterOutputPortCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetOutputPort() const;
+
+  void SetOutputPort(int outputPort);
+
+  std::unique_ptr<CallbackStore> RegisterLengthCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  int GetLength() const;
+
+  void SetLength(int length);
+
+  std::unique_ptr<CallbackStore> RegisterRunningCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetRunning() const;
+
+  void SetRunning(bool running);
+
+  std::unique_ptr<CallbackStore> RegisterDataCallback(NotifyCallback callback,
+                                                      bool initialNotify);
+
+  int GetData(struct HAL_AddressableLEDData* data) const;
+
+  void SetData(struct HAL_AddressableLEDData* data, int length);
+
+ private:
+  explicit AddressableLEDSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h
new file mode 100644
index 0000000..aef979a
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/SimDevice.h>
+#include <units/angle.h>
+
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+class AnalogEncoder;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog encoder.
+ */
+class AnalogEncoderSim {
+ public:
+  /**
+   * Constructs from an AnalogEncoder object.
+   *
+   * @param encoder AnalogEncoder to simulate
+   */
+  explicit AnalogEncoderSim(const AnalogEncoder& encoder);
+
+  /**
+   * Set the position using an {@link Rotation2d}.
+   *
+   * @param angle The angle.
+   */
+  void SetPosition(Rotation2d angle);
+
+  /**
+   * Set the position of the encoder.
+   *
+   * @param turns The position.
+   */
+  void SetTurns(units::turn_t turns);
+
+  /**
+   * Get the simulated position.
+   */
+  units::turn_t GetTurns();
+
+  /**
+   * Get the position as a {@link Rotation2d}.
+   */
+  Rotation2d GetPosition();
+
+ private:
+  hal::SimDouble m_positionSim;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogGyroSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogGyroSim.h
new file mode 100644
index 0000000..685e3ed
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AnalogGyroSim.h
@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class AnalogGyro;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog gyro.
+ */
+class AnalogGyroSim {
+ public:
+  /**
+   * Constructs from an AnalogGyro object.
+   *
+   * @param gyro AnalogGyro to simulate
+   */
+  explicit AnalogGyroSim(const AnalogGyro& gyro);
+
+  /**
+   * Constructs from an analog input channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit AnalogGyroSim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterAngleCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  double GetAngle() const;
+
+  void SetAngle(double angle);
+
+  std::unique_ptr<CallbackStore> RegisterRateCallback(NotifyCallback callback,
+                                                      bool initialNotify);
+
+  double GetRate() const;
+
+  void SetRate(double rate);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogInputSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogInputSim.h
new file mode 100644
index 0000000..a52cec4
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AnalogInputSim.h
@@ -0,0 +1,108 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class AnalogInput;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog input.
+ */
+class AnalogInputSim {
+ public:
+  /**
+   * Constructs from an AnalogInput object.
+   *
+   * @param analogInput AnalogInput to simulate
+   */
+  explicit AnalogInputSim(const AnalogInput& analogInput);
+
+  /**
+   * Constructs from an analog input channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit AnalogInputSim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterAverageBitsCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetAverageBits() const;
+
+  void SetAverageBits(int averageBits);
+
+  std::unique_ptr<CallbackStore> RegisterOversampleBitsCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetOversampleBits() const;
+
+  void SetOversampleBits(int oversampleBits);
+
+  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetVoltage() const;
+
+  void SetVoltage(double voltage);
+
+  std::unique_ptr<CallbackStore> RegisterAccumulatorInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetAccumulatorInitialized() const;
+
+  void SetAccumulatorInitialized(bool accumulatorInitialized);
+
+  std::unique_ptr<CallbackStore> RegisterAccumulatorValueCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int64_t GetAccumulatorValue() const;
+
+  void SetAccumulatorValue(int64_t accumulatorValue);
+
+  std::unique_ptr<CallbackStore> RegisterAccumulatorCountCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int64_t GetAccumulatorCount() const;
+
+  void SetAccumulatorCount(int64_t accumulatorCount);
+
+  std::unique_ptr<CallbackStore> RegisterAccumulatorCenterCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetAccumulatorCenter() const;
+
+  void SetAccumulatorCenter(int accumulatorCenter);
+
+  std::unique_ptr<CallbackStore> RegisterAccumulatorDeadbandCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetAccumulatorDeadband() const;
+
+  void SetAccumulatorDeadband(int accumulatorDeadband);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogOutputSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogOutputSim.h
new file mode 100644
index 0000000..e468fa5
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AnalogOutputSim.h
@@ -0,0 +1,59 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class AnalogOutput;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog output.
+ */
+class AnalogOutputSim {
+ public:
+  /**
+   * Constructs from an AnalogOutput object.
+   *
+   * @param analogOutput AnalogOutput to simulate
+   */
+  explicit AnalogOutputSim(const AnalogOutput& analogOutput);
+
+  /**
+   * Constructs from an analog output channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit AnalogOutputSim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetVoltage() const;
+
+  void SetVoltage(double voltage);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogTriggerSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogTriggerSim.h
new file mode 100644
index 0000000..60d62e1
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/AnalogTriggerSim.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class AnalogTrigger;
+
+namespace sim {
+
+/**
+ * Class to control a simulated analog trigger.
+ */
+class AnalogTriggerSim {
+ public:
+  /**
+   * Constructs from an AnalogTrigger object.
+   *
+   * @param analogTrigger AnalogTrigger to simulate
+   */
+  explicit AnalogTriggerSim(const AnalogTrigger& analogTrigger);
+
+  /**
+   * Creates an AnalogTriggerSim for an analog input channel.
+   *
+   * @param channel analog input channel
+   * @return Simulated object
+   * @throws std::out_of_range if no AnalogTrigger is configured for that
+   *         channel
+   */
+  static AnalogTriggerSim CreateForChannel(int channel);
+
+  /**
+   * Creates an AnalogTriggerSim for a simulated index.
+   * The index is incremented for each simulated AnalogTrigger.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static AnalogTriggerSim CreateForIndex(int index);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterTriggerLowerBoundCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetTriggerLowerBound() const;
+
+  void SetTriggerLowerBound(double triggerLowerBound);
+
+  std::unique_ptr<CallbackStore> RegisterTriggerUpperBoundCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetTriggerUpperBound() const;
+
+  void SetTriggerUpperBound(double triggerUpperBound);
+
+  void ResetData();
+
+ private:
+  explicit AnalogTriggerSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/BatterySim.h b/wpilibc/src/main/native/include/frc/simulation/BatterySim.h
new file mode 100644
index 0000000..ef8fb5d
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/BatterySim.h
@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <numeric>
+
+#include <units/current.h>
+#include <units/impedance.h>
+#include <units/voltage.h>
+
+namespace frc::sim {
+
+class BatterySim {
+ public:
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery
+   * voltage, which can then be retrieved with the {@link
+   * RobotController#getBatteryVoltage()} method.
+   *
+   * @param nominalVoltage The nominal battery voltage. Usually 12v.
+   * @param resistance     The forward resistance of the battery. Most batteries
+   *                       are at or below 20 milliohms.
+   * @param currents       The currents drawn from the battery.
+   * @return The battery's voltage under load.
+   */
+  static units::volt_t Calculate(
+      units::volt_t nominalVoltage, units::ohm_t resistance,
+      std::initializer_list<units::ampere_t> currents) {
+    return nominalVoltage -
+           std::accumulate(currents.begin(), currents.end(), 0_A) * resistance;
+  }
+
+  /**
+   * Calculate the loaded battery voltage. Use this with
+   * {@link RoboRioSim#setVInVoltage(double)} to set the simulated battery
+   * voltage, which can then be retrieved with the {@link
+   * RobotController#getBatteryVoltage()} method. This function assumes a
+   * nominal voltage of 12v and a resistance of 20 milliohms (0.020 ohms)
+   *
+   * @param currents The currents drawn from the battery.
+   * @return The battery's voltage under load.
+   */
+  static units::volt_t Calculate(
+      std::initializer_list<units::ampere_t> currents) {
+    return Calculate(12_V, 0.02_Ohm, currents);
+  }
+};
+
+}  // namespace frc::sim
diff --git a/wpilibc/src/main/native/include/frc/simulation/BuiltInAccelerometerSim.h b/wpilibc/src/main/native/include/frc/simulation/BuiltInAccelerometerSim.h
new file mode 100644
index 0000000..49a581c
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/BuiltInAccelerometerSim.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/Accelerometer.h>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class BuiltInAccelerometer;
+
+namespace sim {
+
+/**
+ * Class to control a simulated built-in accelerometer.
+ */
+class BuiltInAccelerometerSim {
+ public:
+  /**
+   * Constructs for the first built-in accelerometer.
+   */
+  BuiltInAccelerometerSim();
+
+  /**
+   * Constructs from a BuiltInAccelerometer object.
+   *
+   * @param accel BuiltInAccelerometer to simulate
+   */
+  explicit BuiltInAccelerometerSim(const BuiltInAccelerometer& accel);
+
+  std::unique_ptr<CallbackStore> RegisterActiveCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  bool GetActive() const;
+
+  void SetActive(bool active);
+
+  std::unique_ptr<CallbackStore> RegisterRangeCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  HAL_AccelerometerRange GetRange() const;
+
+  void SetRange(HAL_AccelerometerRange range);
+
+  std::unique_ptr<CallbackStore> RegisterXCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetX() const;
+
+  void SetX(double x);
+
+  std::unique_ptr<CallbackStore> RegisterYCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetY() const;
+
+  void SetY(double y);
+
+  std::unique_ptr<CallbackStore> RegisterZCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetZ() const;
+
+  void SetZ(double z);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/CallbackStore.h b/wpilibc/src/main/native/include/frc/simulation/CallbackStore.h
new file mode 100644
index 0000000..379296a
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/CallbackStore.h
@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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 <hal/Value.h>
+#include <wpi/StringRef.h>
+
+namespace frc {
+namespace sim {
+
+using NotifyCallback = std::function<void(wpi::StringRef, const HAL_Value*)>;
+using ConstBufferCallback = std::function<void(
+    wpi::StringRef, const unsigned char* buffer, unsigned int count)>;
+typedef void (*CancelCallbackFunc)(int32_t index, int32_t uid);
+typedef void (*CancelCallbackNoIndexFunc)(int32_t uid);
+typedef void (*CancelCallbackChannelFunc)(int32_t index, int32_t channel,
+                                          int32_t uid);
+
+void CallbackStoreThunk(const char* name, void* param, const HAL_Value* value);
+void ConstBufferCallbackStoreThunk(const char* name, void* param,
+                                   const unsigned char* buffer,
+                                   unsigned int count);
+
+class CallbackStore {
+ public:
+  CallbackStore(int32_t i, NotifyCallback cb, CancelCallbackNoIndexFunc ccf);
+
+  CallbackStore(int32_t i, int32_t u, NotifyCallback cb,
+                CancelCallbackFunc ccf);
+
+  CallbackStore(int32_t i, int32_t c, int32_t u, NotifyCallback cb,
+                CancelCallbackChannelFunc ccf);
+
+  CallbackStore(int32_t i, ConstBufferCallback cb,
+                CancelCallbackNoIndexFunc ccf);
+
+  CallbackStore(int32_t i, int32_t u, ConstBufferCallback cb,
+                CancelCallbackFunc ccf);
+
+  CallbackStore(int32_t i, int32_t c, int32_t u, ConstBufferCallback cb,
+                CancelCallbackChannelFunc ccf);
+
+  CallbackStore(const CallbackStore&) = delete;
+  CallbackStore& operator=(const CallbackStore&) = delete;
+
+  ~CallbackStore();
+
+  void SetUid(int32_t uid);
+
+  friend void CallbackStoreThunk(const char* name, void* param,
+                                 const HAL_Value* value);
+
+  friend void ConstBufferCallbackStoreThunk(const char* name, void* param,
+                                            const unsigned char* buffer,
+                                            unsigned int count);
+
+ private:
+  int32_t index;
+  int32_t channel;
+  int32_t uid;
+
+  NotifyCallback callback;
+  ConstBufferCallback constBufferCallback;
+  union {
+    CancelCallbackFunc ccf;
+    CancelCallbackChannelFunc cccf;
+    CancelCallbackNoIndexFunc ccnif;
+  };
+  enum CancelType { Normal, Channel, NoIndex };
+  CancelType cancelType;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/DIOSim.h b/wpilibc/src/main/native/include/frc/simulation/DIOSim.h
new file mode 100644
index 0000000..f6edb39
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DIOSim.h
@@ -0,0 +1,88 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class DigitalInput;
+class DigitalOutput;
+
+namespace sim {
+
+/**
+ * Class to control a simulated digital input or output.
+ */
+class DIOSim {
+ public:
+  /**
+   * Constructs from a DigitalInput object.
+   *
+   * @param input DigitalInput to simulate
+   */
+  explicit DIOSim(const DigitalInput& input);
+
+  /**
+   * Constructs from a DigitalOutput object.
+   *
+   * @param output DigitalOutput to simulate
+   */
+  explicit DIOSim(const DigitalOutput& output);
+
+  /**
+   * Constructs from an digital I/O channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit DIOSim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterValueCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  bool GetValue() const;
+
+  void SetValue(bool value);
+
+  std::unique_ptr<CallbackStore> RegisterPulseLengthCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetPulseLength() const;
+
+  void SetPulseLength(double pulseLength);
+
+  std::unique_ptr<CallbackStore> RegisterIsInputCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetIsInput() const;
+
+  void SetIsInput(bool isInput);
+
+  std::unique_ptr<CallbackStore> RegisterFilterIndexCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetFilterIndex() const;
+
+  void SetFilterIndex(int filterIndex);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
new file mode 100644
index 0000000..a75edb9
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
@@ -0,0 +1,235 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/kinematics/DifferentialDriveKinematics.h>
+#include <frc/system/LinearSystem.h>
+#include <frc/system/plant/DCMotor.h>
+
+#include <Eigen/Core>
+#include <units/length.h>
+#include <units/moment_of_inertia.h>
+#include <units/time.h>
+#include <units/voltage.h>
+
+namespace frc::sim {
+
+class DifferentialDrivetrainSim {
+ public:
+  /**
+   * Create a SimDrivetrain.
+   *
+   * @param drivetrainPlant   The LinearSystem representing the robot's
+   * drivetrain. This system can be created with
+   * LinearSystemId#createDrivetrainVelocitySystem or
+   * LinearSystemId#identifyDrivetrainSystem.
+   * @param trackWidth        The robot's track width.
+   * @param driveMotor        A {@link DCMotor} representing the left side of
+   * the drivetrain.
+   * @param gearingRatio      The gearingRatio ratio of the left side, as output
+   * over input. This must be the same ratio as the ratio used to identify or
+   * create the drivetrainPlant.
+   * @param wheelRadiusMeters The radius of the wheels on the drivetrain, in
+   * meters.
+   */
+  DifferentialDrivetrainSim(const LinearSystem<2, 2, 2>& plant,
+                            units::meter_t trackWidth, DCMotor driveMotor,
+                            double gearingRatio, units::meter_t wheelRadius);
+
+  /**
+   * Create a SimDrivetrain.
+   *
+   * @param driveMotor  A {@link DCMotor} representing the left side of the
+   * drivetrain.
+   * @param gearing     The gearing on the drive between motor and wheel, as
+   * output over input. This must be the same ratio as the ratio used to
+   * identify or create the drivetrainPlant.
+   * @param J           The moment of inertia of the drivetrain about its
+   * center.
+   * @param mass        The mass of the drivebase.
+   * @param wheelRadius The radius of the wheels on the drivetrain.
+   * @param trackWidth  The robot's track width, or distance between left and
+   * right wheels.
+   */
+  DifferentialDrivetrainSim(frc::DCMotor driveMotor, double gearing,
+                            units::kilogram_square_meter_t J,
+                            units::kilogram_t mass, units::meter_t wheelRadius,
+                            units::meter_t trackWidth);
+
+  /**
+   * Sets the applied voltage to the drivetrain. Note that positive voltage must
+   * make that side of the drivetrain travel forward (+X).
+   *
+   * @param leftVoltage  The left voltage.
+   * @param rightVoltage The right voltage.
+   */
+  void SetInputs(units::volt_t leftVoltage, units::volt_t rightVoltage);
+
+  /**
+   * Sets the gearing reduction on the drivetrain. This is commonly used for
+   * shifting drivetrains.
+   *
+   * @param newGearing The new gear ratio, as output over input.
+   */
+  void SetGearing(double newGearing);
+
+  /**
+   * Updates the simulation.
+   *
+   * @param dt The time that's passed since the last {@link #update(double)}
+   * call.
+   */
+  void Update(units::second_t dt);
+
+  /**
+   * Returns an element of the state vector.
+   *
+   * @param state The row of the state vector.
+   */
+  double GetState(int state) const;
+
+  /**
+   * Returns the current gearing reduction of the drivetrain, as output over
+   * input.
+   */
+  double GetGearing() const;
+
+  /**
+   * Returns the current state vector x.
+   */
+  Eigen::Matrix<double, 7, 1> GetState() const;
+
+  /**
+   * Returns the direction the robot is pointing.
+   *
+   * Note that this angle is counterclockwise-positive, while most gyros are
+   * clockwise positive.
+   */
+  Rotation2d GetHeading() const;
+
+  /**
+   * Returns the current pose.
+   */
+  Pose2d GetPose() const;
+
+  /**
+   * Returns the currently drawn current.
+   */
+  units::ampere_t GetCurrentDraw() const;
+
+  /**
+   * Sets the system state.
+   *
+   * @param state The state.
+   */
+  void SetState(const Eigen::Matrix<double, 7, 1>& state);
+
+  /**
+   * Sets the system pose.
+   *
+   * @param pose The pose.
+   */
+  void SetPose(const frc::Pose2d& pose);
+
+  Eigen::Matrix<double, 7, 1> Dynamics(const Eigen::Matrix<double, 7, 1>& x,
+                                       const Eigen::Matrix<double, 2, 1>& u);
+
+  class State {
+   public:
+    static constexpr int kX = 0;
+    static constexpr int kY = 1;
+    static constexpr int kHeading = 2;
+    static constexpr int kLeftVelocity = 3;
+    static constexpr int kRightVelocity = 4;
+    static constexpr int kLeftPosition = 5;
+    static constexpr int kRightPosition = 6;
+  };
+
+  /**
+   * Represents a gearing option of the Toughbox mini.
+   * 12.75:1 -- 14:50 and 14:50
+   * 10.71:1 -- 14:50 and 16:48
+   * 8.45:1 -- 14:50 and 19:45
+   * 7.31:1 -- 14:50 and 21:43
+   * 5.95:1 -- 14:50 and 24:40
+   */
+  class KitbotGearing {
+   public:
+    static constexpr double k12p75 = 12.75;
+    static constexpr double k10p71 = 10.71;
+    static constexpr double k8p45 = 8.45;
+    static constexpr double k7p31 = 7.31;
+    static constexpr double k5p95 = 5.95;
+  };
+
+  class KitbotMotor {
+   public:
+    static constexpr frc::DCMotor SingleCIMPerSide = frc::DCMotor::CIM(1);
+    static constexpr frc::DCMotor DualCIMPerSide = frc::DCMotor::CIM(2);
+    static constexpr frc::DCMotor SingleMiniCIMPerSide =
+        frc::DCMotor::MiniCIM(1);
+    static constexpr frc::DCMotor DualMiniCIMPerSide = frc::DCMotor::MiniCIM(2);
+  };
+
+  class KitbotWheelSize {
+   public:
+    static constexpr units::meter_t kSixInch = 6_in;
+    static constexpr units::meter_t kEightInch = 8_in;
+    static constexpr units::meter_t kTenInch = 10_in;
+  };
+
+  /**
+   * Create a sim for the standard FRC kitbot.
+   *
+   * @param motor     The motors installed in the bot.
+   * @param gearing   The gearing reduction used.
+   * @param wheelSize The wheel size.
+   */
+  static DifferentialDrivetrainSim CreateKitbotSim(frc::DCMotor motor,
+                                                   double gearing,
+                                                   units::meter_t wheelSize) {
+    // MOI estimation -- note that I = m r^2 for point masses
+    units::kilogram_square_meter_t batteryMoi = 12.5_lb * 10_in * 10_in;
+    units::kilogram_square_meter_t gearboxMoi = (2.8_lb + 2.0_lb) *
+                                                2  // CIM plus toughbox per side
+                                                * (26_in / 2) * (26_in / 2);
+
+    return DifferentialDrivetrainSim{
+        motor, gearing, batteryMoi + gearboxMoi, 25_kg, wheelSize / 2.0, 26_in};
+  }
+
+  /**
+   * Create a sim for the standard FRC kitbot.
+   *
+   * @param motor     The motors installed in the bot.
+   * @param gearing   The gearing reduction used.
+   * @param wheelSize The wheel size.
+   * @param J         The moment of inertia of the drivebase. This can be
+   * calculated using frc-characterization.
+   */
+  static DifferentialDrivetrainSim CreateKitbotSim(
+      frc::DCMotor motor, double gearing, units::meter_t wheelSize,
+      units::kilogram_square_meter_t J) {
+    return DifferentialDrivetrainSim{motor, gearing,         J,
+                                     25_kg, wheelSize / 2.0, 26_in};
+  }
+
+ private:
+  LinearSystem<2, 2, 2> m_plant;
+  units::meter_t m_rb;
+  units::meter_t m_wheelRadius;
+
+  DCMotor m_motor;
+
+  double m_originalGearing;
+  double m_currentGearing;
+
+  Eigen::Matrix<double, 7, 1> m_x;
+  Eigen::Matrix<double, 2, 1> m_u;
+};
+}  // namespace frc::sim
diff --git a/wpilibc/src/main/native/include/frc/simulation/DigitalPWMSim.h b/wpilibc/src/main/native/include/frc/simulation/DigitalPWMSim.h
new file mode 100644
index 0000000..2ff8a12
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DigitalPWMSim.h
@@ -0,0 +1,82 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class DigitalOutput;
+
+namespace sim {
+
+/**
+ * Class to control a simulated digital PWM output.
+ *
+ * This is for duty cycle PWM outputs on a DigitalOutput, not for the servo
+ * style PWM outputs on a PWM channel.
+ */
+class DigitalPWMSim {
+ public:
+  /**
+   * Constructs from a DigitalOutput object.
+   *
+   * @param digitalOutput DigitalOutput to simulate
+   */
+  explicit DigitalPWMSim(const DigitalOutput& digitalOutput);
+
+  /**
+   * Creates an DigitalPWMSim for a digital I/O channel.
+   *
+   * @param channel DIO channel
+   * @return Simulated object
+   * @throws std::out_of_range if no Digital PWM is configured for that channel
+   */
+  static DigitalPWMSim CreateForChannel(int channel);
+
+  /**
+   * Creates an DigitalPWMSim for a simulated index.
+   * The index is incremented for each simulated DigitalPWM.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static DigitalPWMSim CreateForIndex(int index);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterDutyCycleCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetDutyCycle() const;
+
+  void SetDutyCycle(double dutyCycle);
+
+  std::unique_ptr<CallbackStore> RegisterPinCallback(NotifyCallback callback,
+                                                     bool initialNotify);
+
+  int GetPin() const;
+
+  void SetPin(int pin);
+
+  void ResetData();
+
+ private:
+  explicit DigitalPWMSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/DriverStationSim.h b/wpilibc/src/main/native/include/frc/simulation/DriverStationSim.h
new file mode 100644
index 0000000..80cf942
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DriverStationSim.h
@@ -0,0 +1,248 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/DriverStationTypes.h>
+
+#include "frc/DriverStation.h"
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+namespace sim {
+
+/**
+ * Class to control a simulated driver station.
+ */
+class DriverStationSim {
+ public:
+  static std::unique_ptr<CallbackStore> RegisterEnabledCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetEnabled();
+
+  static void SetEnabled(bool enabled);
+
+  static std::unique_ptr<CallbackStore> RegisterAutonomousCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetAutonomous();
+
+  static void SetAutonomous(bool autonomous);
+
+  static std::unique_ptr<CallbackStore> RegisterTestCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetTest();
+
+  static void SetTest(bool test);
+
+  static std::unique_ptr<CallbackStore> RegisterEStopCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetEStop();
+
+  static void SetEStop(bool eStop);
+
+  static std::unique_ptr<CallbackStore> RegisterFmsAttachedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetFmsAttached();
+
+  static void SetFmsAttached(bool fmsAttached);
+
+  static std::unique_ptr<CallbackStore> RegisterDsAttachedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetDsAttached();
+
+  static void SetDsAttached(bool dsAttached);
+
+  static std::unique_ptr<CallbackStore> RegisterAllianceStationIdCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static HAL_AllianceStationID GetAllianceStationId();
+
+  static void SetAllianceStationId(HAL_AllianceStationID allianceStationId);
+
+  static std::unique_ptr<CallbackStore> RegisterMatchTimeCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static double GetMatchTime();
+
+  static void SetMatchTime(double matchTime);
+
+  /**
+   * Updates DriverStation data so that new values are visible to the user
+   * program.
+   */
+  static void NotifyNewData();
+
+  /**
+   * Sets suppression of DriverStation::ReportError and ReportWarning messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  static void SetSendError(bool shouldSend);
+
+  /**
+   * Sets suppression of DriverStation::SendConsoleLine messages.
+   *
+   * @param shouldSend If false then messages will be suppressed.
+   */
+  static void SetSendConsoleLine(bool shouldSend);
+
+  /**
+   * Gets the joystick outputs.
+   *
+   * @param stick The joystick number
+   * @return The joystick outputs
+   */
+  static int64_t GetJoystickOutputs(int stick);
+
+  /**
+   * Gets the joystick rumble.
+   *
+   * @param stick The joystick number
+   * @param rumbleNum Rumble to get (0=left, 1=right)
+   * @return The joystick rumble value
+   */
+  static int GetJoystickRumble(int stick, int rumbleNum);
+
+  /**
+   * Sets the state of one joystick button. Button indexes begin at 1.
+   *
+   * @param stick The joystick number
+   * @param button The button index, beginning at 1
+   * @param state The state of the joystick button
+   */
+  static void SetJoystickButton(int stick, int button, bool state);
+
+  /**
+   * Gets the value of the axis on a joystick.
+   *
+   * @param stick The joystick number
+   * @param axis The analog axis number
+   * @param value The value of the axis on the joystick
+   */
+  static void SetJoystickAxis(int stick, int axis, double value);
+
+  /**
+   * Gets the state of a POV on a joystick.
+   *
+   * @param stick The joystick number
+   * @param pov The POV number
+   * @param value the angle of the POV in degrees, or -1 for not pressed
+   */
+  static void SetJoystickPOV(int stick, int pov, int value);
+
+  /**
+   * Sets the state of all the buttons on a joystick.
+   *
+   * @param stick The joystick number
+   * @param buttons The bitmap state of the buttons on the joystick
+   */
+  static void SetJoystickButtons(int stick, uint32_t buttons);
+
+  /**
+   * Sets the number of axes for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of axes on the indicated joystick
+   */
+  static void SetJoystickAxisCount(int stick, int count);
+
+  /**
+   * Sets the number of POVs for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of POVs on the indicated joystick
+   */
+  static void SetJoystickPOVCount(int stick, int count);
+
+  /**
+   * Sets the number of buttons for a joystick.
+   *
+   * @param stick The joystick number
+   * @param count The number of buttons on the indicated joystick
+   */
+  static void SetJoystickButtonCount(int stick, int count);
+
+  /**
+   * Sets the value of isXbox for a joystick.
+   *
+   * @param stick The joystick number
+   * @param isXbox The value of isXbox
+   */
+  static void SetJoystickIsXbox(int stick, bool isXbox);
+
+  /**
+   * Sets the value of type for a joystick.
+   *
+   * @param stick The joystick number
+   * @param type The value of type
+   */
+  static void SetJoystickType(int stick, int type);
+
+  /**
+   * Sets the name of a joystick.
+   *
+   * @param stick The joystick number
+   * @param name The value of name
+   */
+  static void SetJoystickName(int stick, const char* name);
+
+  /**
+   * Sets the types of Axes for a joystick.
+   *
+   * @param stick The joystick number
+   * @param axis The target axis
+   * @param type The type of axis
+   */
+  static void SetJoystickAxisType(int stick, int axis, int type);
+
+  /**
+   * Sets the game specific message.
+   *
+   * @param message the game specific message
+   */
+  static void SetGameSpecificMessage(const char* message);
+
+  /**
+   * Sets the event name.
+   *
+   * @param name the event name
+   */
+  static void SetEventName(const char* name);
+
+  /**
+   * Sets the match type.
+   *
+   * @param type the match type
+   */
+  static void SetMatchType(DriverStation::MatchType type);
+
+  /**
+   * Sets the match number.
+   *
+   * @param matchNumber the match number
+   */
+  static void SetMatchNumber(int matchNumber);
+
+  /**
+   * Sets the replay number.
+   *
+   * @param replayNumber the replay number
+   */
+  static void SetReplayNumber(int replayNumber);
+
+  static void ResetData();
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h
new file mode 100644
index 0000000..5d9a039
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h
@@ -0,0 +1,49 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/SimDevice.h>
+#include <units/angle.h>
+
+namespace frc {
+
+class DutyCycleEncoder;
+
+namespace sim {
+
+/**
+ * Class to control a simulated duty cycle encoder.
+ */
+class DutyCycleEncoderSim {
+ public:
+  /**
+   * Constructs from a DutyCycleEncoder object.
+   *
+   * @param dutyCycleEncoder DutyCycleEncoder to simulate
+   */
+  explicit DutyCycleEncoderSim(const DutyCycleEncoder& encoder);
+
+  /**
+   * Set the position tin turns.
+   *
+   * @param turns The position.
+   */
+  void Set(units::turn_t turns);
+
+  /**
+   * Set the position.
+   */
+  void SetDistance(double distance);
+
+ private:
+  hal::SimDouble m_simPosition;
+  hal::SimDouble m_simDistancePerRotation;
+};
+
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/DutyCycleSim.h b/wpilibc/src/main/native/include/frc/simulation/DutyCycleSim.h
new file mode 100644
index 0000000..40dad13
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/DutyCycleSim.h
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class DutyCycle;
+
+namespace sim {
+
+/**
+ * Class to control a simulated duty cycle digital input.
+ */
+class DutyCycleSim {
+ public:
+  /**
+   * Constructs from a DutyCycle object.
+   *
+   * @param dutyCycle DutyCycle to simulate
+   */
+  explicit DutyCycleSim(const DutyCycle& dutyCycle);
+
+  /**
+   * Creates a DutyCycleSim for a digital input channel.
+   *
+   * @param channel digital input channel
+   * @return Simulated object
+   * @throws std::out_of_range if no DutyCycle is configured for that channel
+   */
+  static DutyCycleSim CreateForChannel(int channel);
+
+  /**
+   * Creates a DutyCycleSim for a simulated index.
+   * The index is incremented for each simulated DutyCycle.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static DutyCycleSim CreateForIndex(int index);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterFrequencyCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetFrequency() const;
+
+  void SetFrequency(int count);
+
+  std::unique_ptr<CallbackStore> RegisterOutputCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  double GetOutput() const;
+
+  void SetOutput(double period);
+
+  void ResetData();
+
+ private:
+  explicit DutyCycleSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
new file mode 100644
index 0000000..9b7069f
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <array>
+
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/velocity.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated elevator mechanism.
+ */
+class ElevatorSim : public LinearSystemSim<2, 1, 1> {
+ public:
+  /**
+   * Constructs a simulated elevator mechanism.
+   *
+   * @param plant              The linear system that represents the elevator.
+   * @param gearbox            The type of and number of motors in your
+   *                           elevator gearbox.
+   * @param gearing            The gearing of the elevator (numbers greater
+   *                           than 1 represent reductions).
+   * @param drumRadius         The radius of the drum that your cable is
+   *                           wrapped around.
+   * @param minHeight          The minimum allowed height of the elevator.
+   * @param maxHeight          The maximum allowed height of the elevator.
+   * @param measurementStdDevs The standard deviation of the measurements.
+   */
+  ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox,
+              double gearing, units::meter_t drumRadius,
+              units::meter_t minHeight, units::meter_t maxHeight,
+              const std::array<double, 1>& measurementStdDevs = {0.0});
+
+  /**
+   * Constructs a simulated elevator mechanism.
+   *
+   * @param gearbox            The type of and number of motors in your
+   *                           elevator gearbox.
+   * @param gearing            The gearing of the elevator (numbers greater
+   *                           than 1 represent reductions).
+   * @param carriageMass       The mass of the elevator carriage.
+   * @param drumRadius         The radius of the drum that your cable is
+   *                           wrapped around.
+   * @param minHeight          The minimum allowed height of the elevator.
+   * @param maxHeight          The maximum allowed height of the elevator.
+   * @param measurementStdDevs The standard deviation of the measurements.
+   */
+  ElevatorSim(const DCMotor& gearbox, double gearing,
+              units::kilogram_t carriageMass, units::meter_t drumRadius,
+              units::meter_t minHeight, units::meter_t maxHeight,
+              const std::array<double, 1>& measurementStdDevs = {0.0});
+
+  /**
+   * Returns whether the elevator has hit the lower limit.
+   *
+   * @param x The current elevator state.
+   * @return Whether the elevator has hit the lower limit.
+   */
+  bool HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const;
+
+  /**
+   * Returns whether the elevator has hit the upper limit.
+   *
+   * @param x The current elevator state.
+   * @return Whether the elevator has hit the upper limit.
+   */
+  bool HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const;
+
+  /**
+   * Returns the position of the elevator.
+   *
+   * @return The position of the elevator.
+   */
+  units::meter_t GetPosition() const;
+
+  /**
+   * Returns the velocity of the elevator.
+   *
+   * @return The velocity of the elevator.
+   */
+  units::meters_per_second_t GetVelocity() const;
+
+  /**
+   * Returns the elevator current draw.
+   *
+   * @return The elevator current draw.
+   */
+  units::ampere_t GetCurrentDraw() const override;
+
+  /**
+   * Sets the input voltage for the elevator.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+ protected:
+  /**
+   * Updates the state estimate of the elevator.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (voltage).
+   * @param dt          The time difference between controller updates.
+   */
+  Eigen::Matrix<double, 2, 1> UpdateX(
+      const Eigen::Matrix<double, 2, 1>& currentXhat,
+      const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
+
+ private:
+  DCMotor m_gearbox;
+  units::meter_t m_drumRadius;
+  units::meter_t m_minHeight;
+  units::meter_t m_maxHeight;
+  double m_gearing;
+};
+}  // namespace frc::sim
diff --git a/wpilibc/src/main/native/include/frc/simulation/EncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/EncoderSim.h
new file mode 100644
index 0000000..4049ab8
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/EncoderSim.h
@@ -0,0 +1,130 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class Encoder;
+
+namespace sim {
+
+/**
+ * Class to control a simulated encoder.
+ */
+class EncoderSim {
+ public:
+  /**
+   * Constructs from an Encoder object.
+   *
+   * @param encoder Encoder to simulate
+   */
+  explicit EncoderSim(const Encoder& encoder);
+
+  /**
+   * Creates an EncoderSim for a digital input channel.  Encoders take two
+   * channels, so either one may be specified.
+   *
+   * @param channel digital input channel
+   * @return Simulated object
+   * @throws NoSuchElementException if no Encoder is configured for that channel
+   */
+  static EncoderSim CreateForChannel(int channel);
+
+  /**
+   * Creates an EncoderSim for a simulated index.
+   * The index is incremented for each simulated Encoder.
+   *
+   * @param index simulator index
+   * @return Simulated object
+   */
+  static EncoderSim CreateForIndex(int index);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterCountCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  int GetCount() const;
+
+  void SetCount(int count);
+
+  std::unique_ptr<CallbackStore> RegisterPeriodCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  double GetPeriod() const;
+
+  void SetPeriod(double period);
+
+  std::unique_ptr<CallbackStore> RegisterResetCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  bool GetReset() const;
+
+  void SetReset(bool reset);
+
+  std::unique_ptr<CallbackStore> RegisterMaxPeriodCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetMaxPeriod() const;
+
+  void SetMaxPeriod(double maxPeriod);
+
+  std::unique_ptr<CallbackStore> RegisterDirectionCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetDirection() const;
+
+  void SetDirection(bool direction);
+
+  std::unique_ptr<CallbackStore> RegisterReverseDirectionCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetReverseDirection() const;
+
+  void SetReverseDirection(bool reverseDirection);
+
+  std::unique_ptr<CallbackStore> RegisterSamplesToAverageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetSamplesToAverage() const;
+
+  void SetSamplesToAverage(int samplesToAverage);
+
+  std::unique_ptr<CallbackStore> RegisterDistancePerPulseCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetDistancePerPulse() const;
+
+  void SetDistancePerPulse(double distancePerPulse);
+
+  void ResetData();
+
+  void SetDistance(double distance);
+
+  double GetDistance();
+
+  void SetRate(double rate);
+
+  double GetRate();
+
+ private:
+  explicit EncoderSim(int index) : m_index{index} {}
+
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/Field2d.h b/wpilibc/src/main/native/include/frc/simulation/Field2d.h
new file mode 100644
index 0000000..de30847
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/Field2d.h
@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/SimDevice.h>
+#include <units/length.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+/**
+ * 2D representation of game field (for simulation).
+ *
+ * In non-simulation mode this simply stores and returns the robot pose.
+ *
+ * The robot pose is the actual location shown on the simulation view.
+ * This may or may not match the robot's internal odometry.  For example, if
+ * the robot is shown at a particular starting location, the pose in this
+ * class would represent the actual location on the field, but the robot's
+ * internal state might have a 0,0,0 pose (unless it's initialized to
+ * something different).
+ *
+ * As the user is able to edit the pose, code performing updates should get
+ * the robot pose, transform it as appropriate (e.g. based on simulated wheel
+ * velocity), and set the new pose.
+ */
+class Field2d {
+ public:
+  Field2d();
+
+  /**
+   * Set the robot pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  void SetRobotPose(const Pose2d& pose);
+
+  /**
+   * Set the robot pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  void SetRobotPose(units::meter_t x, units::meter_t y, Rotation2d rotation);
+
+  /**
+   * Get the robot pose.
+   *
+   * @return 2D pose
+   */
+  Pose2d GetRobotPose();
+
+ private:
+  Pose2d m_pose;
+
+  hal::SimDevice m_device;
+  hal::SimDouble m_x;
+  hal::SimDouble m_y;
+  hal::SimDouble m_rot;
+};
+
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
new file mode 100644
index 0000000..1c47c45
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/angular_velocity.h>
+#include <units/moment_of_inertia.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/LinearSystem.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated flywheel mechanism.
+ */
+class FlywheelSim : public LinearSystemSim<1, 1, 1> {
+ public:
+  /**
+   * Creates a simulated flywhel mechanism.
+   *
+   * @param plant              The linear system representing the flywheel.
+   * @param gearbox            The type of and number of motors in the flywheel
+   *                           gearbox.
+   * @param gearing            The gearing of the flywheel (numbers greater than
+   *                           1 represent reductions).
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   */
+  FlywheelSim(const LinearSystem<1, 1, 1>& plant, const DCMotor& gearbox,
+              double gearing,
+              const std::array<double, 1>& measurementStdDevs = {0.0});
+
+  /**
+   * Creates a simulated flywhel mechanism.
+   *
+   * @param gearbox            The type of and number of motors in the flywheel
+   *                           gearbox.
+   * @param gearing            The gearing of the flywheel (numbers greater than
+   *                           1 represent reductions).
+   * @param moi                The moment of inertia of the flywheel.
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   */
+  FlywheelSim(const DCMotor& gearbox, double gearing,
+              units::kilogram_square_meter_t moi,
+              const std::array<double, 1>& measurementStdDevs = {0.0});
+
+  /**
+   * Returns the flywheel velocity.
+   *
+   * @return The flywheel velocity.
+   */
+  units::radians_per_second_t GetAngularVelocity() const;
+
+  /**
+   * Returns the flywheel current draw.
+   *
+   * @return The flywheel current draw.
+   */
+  units::ampere_t GetCurrentDraw() const override;
+
+  /**
+   * Sets the input voltage for the flywheel.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+ private:
+  DCMotor m_gearbox;
+  double m_gearing;
+};
+}  // namespace frc::sim
diff --git a/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h b/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h
new file mode 100644
index 0000000..1705200
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h
@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <stdint.h>
+
+#include "frc/GenericHID.h"
+
+namespace frc {
+
+class GenericHID;
+
+namespace sim {
+
+/**
+ * Class to control a simulated generic joystick.
+ */
+class GenericHIDSim {
+ public:
+  /**
+   * Constructs from a GenericHID object.
+   *
+   * @param joystick joystick to simulate
+   */
+  explicit GenericHIDSim(const GenericHID& joystick);
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit GenericHIDSim(int port);
+
+  /**
+   * Updates joystick data so that new values are visible to the user program.
+   */
+  void NotifyNewData();
+
+  void SetRawButton(int button, bool value);
+
+  void SetRawAxis(int axis, double value);
+
+  void SetPOV(int pov, int value);
+
+  void SetPOV(int value);
+
+  void SetAxisCount(int count);
+
+  void SetPOVCount(int count);
+
+  void SetButtonCount(int count);
+
+  void SetType(GenericHID::HIDType type);
+
+  void SetName(const char* name);
+
+  void SetAxisType(int axis, int type);
+
+  bool GetOutput(int outputNumber);
+
+  int64_t GetOutputs();
+
+  double GetRumble(GenericHID::RumbleType type);
+
+ protected:
+  int m_port;
+};
+
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/JoystickSim.h b/wpilibc/src/main/native/include/frc/simulation/JoystickSim.h
new file mode 100644
index 0000000..5735728
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/JoystickSim.h
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/GenericHIDSim.h"
+
+namespace frc {
+
+class Joystick;
+
+namespace sim {
+
+/**
+ * Class to control a simulated joystick.
+ */
+class JoystickSim : public GenericHIDSim {
+ public:
+  /**
+   * Constructs from a Joystick object.
+   *
+   * @param joystick joystick to simulate
+   */
+  explicit JoystickSim(const Joystick& joystick);
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit JoystickSim(int port);
+
+  void SetX(double value);
+
+  void SetY(double value);
+
+  void SetZ(double value);
+
+  void SetTwist(double value);
+
+  void SetThrottle(double value);
+
+  void SetTrigger(bool state);
+
+  void SetTop(bool state);
+
+ private:
+  const Joystick* m_joystick = nullptr;
+};
+
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
new file mode 100644
index 0000000..4f3c658
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
@@ -0,0 +1,137 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <array>
+
+#include <Eigen/Core>
+#include <units/current.h>
+#include <units/time.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/system/LinearSystem.h"
+
+namespace frc::sim {
+/**
+ * This class helps simulate linear systems. To use this class, do the following
+ * in the simulationPeriodic() method.
+ *
+ * Call the SetInput() method with the inputs to your system (generally
+ * voltage). Call the Update() method to update the simulation. Set simulated
+ * sensor readings with the simulated positions in the GetOutput() method.
+ *
+ * @tparam States  The number of states of the system.
+ * @tparam Inputs  The number of inputs to the system.
+ * @tparam Outputs The number of outputs of the system.
+ */
+template <int States, int Inputs, int Outputs>
+class LinearSystemSim {
+ public:
+  /**
+   * Creates a simulated generic linear system.
+   *
+   * @param system             The system to simulate.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   */
+  LinearSystemSim(const LinearSystem<States, Inputs, Outputs>& system,
+                  const std::array<double, Outputs>& measurementStdDevs =
+                      std::array<double, Outputs>{})
+      : m_plant(system), m_measurementStdDevs(measurementStdDevs) {
+    m_x = Eigen::Matrix<double, States, 1>::Zero();
+    m_y = Eigen::Matrix<double, Outputs, 1>::Zero();
+    m_u = Eigen::Matrix<double, Inputs, 1>::Zero();
+  }
+
+  /**
+   * Updates the simulation.
+   *
+   * @param dt The time between updates.
+   */
+  void Update(units::second_t dt) {
+    // Update x. By default, this is the linear system dynamics x_k+1 = Ax_k +
+    // Bu_k
+    m_x = UpdateX(m_x, m_u, dt);
+
+    // y = Cx + Du
+    m_y = m_plant.CalculateY(m_x, m_u);
+
+    // Add noise. If the user did not pass a noise vector to the
+    // constructor, then this method will not do anything because
+    // the standard deviations default to zero.
+    m_y += frc::MakeWhiteNoiseVector<Outputs>(m_measurementStdDevs);
+  }
+
+  /**
+   * Returns the current output of the plant.
+   *
+   * @return The current output of the plant.
+   */
+  const Eigen::Matrix<double, Outputs, 1>& GetOutput() const { return m_y; }
+
+  /**
+   * Returns an element of the current output of the plant.
+   *
+   * @param row The row to return.
+   * @return An element of the current output of the plant.
+   */
+  double GetOutput(int row) const { return m_y(row); }
+
+  /**
+   * Sets the system inputs (usually voltages).
+   *
+   * @param u The system inputs.
+   */
+  void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) { m_u = u; }
+
+  /*
+   * Sets the system inputs.
+   *
+   * @param row   The row in the input matrix to set.
+   * @param value The value to set the row to.
+   */
+  void SetInput(int row, double value) { m_u(row, 0) = value; }
+
+  /**
+   * Sets the system state.
+   *
+   * @param state The new state.
+   */
+  void SetState(const Eigen::Matrix<double, States, 1>& state) { m_x = state; }
+
+  /**
+   * Returns the current drawn by this simulated system. Override this method to
+   * add a custom current calculation.
+   *
+   * @return The current drawn by this simulated mechanism.
+   */
+  virtual units::ampere_t GetCurrentDraw() const {
+    return units::ampere_t(0.0);
+  }
+
+ protected:
+  /**
+   * Updates the state estimate of the system.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (usually voltage).
+   * @param dt          The time difference between controller updates.
+   */
+  virtual Eigen::Matrix<double, States, 1> UpdateX(
+      const Eigen::Matrix<double, States, 1>& currentXhat,
+      const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+    return m_plant.CalculateX(currentXhat, u, dt);
+  }
+
+  LinearSystem<States, Inputs, Outputs> m_plant;
+
+  Eigen::Matrix<double, States, 1> m_x;
+  Eigen::Matrix<double, Outputs, 1> m_y;
+  Eigen::Matrix<double, Inputs, 1> m_u;
+  std::array<double, Outputs> m_measurementStdDevs;
+};
+}  // namespace frc::sim
diff --git a/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h b/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h
new file mode 100644
index 0000000..3d61cf5
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h
@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <map>
+#include <string>
+
+#include <hal/SimDevice.h>
+#include <wpi/StringMap.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+class Mechanism2D {
+ public:
+  Mechanism2D();
+
+  /**
+   * Set/Create the angle of a ligament
+   *
+   * @param ligamentPath json path to the ligament
+   * @param angle to set the ligament
+   */
+  void SetLigamentAngle(const wpi::Twine& ligamentPath, float angle);
+
+  /**
+   * Set/Create the length of a ligament
+   *
+   * @param ligamentPath json path to the ligament
+   * @param length to set the ligament
+   */
+  void SetLigamentLength(const wpi::Twine& ligamentPath, float length);
+
+ private:
+  wpi::StringMap<hal::SimDouble> createdItems;
+  hal::SimDevice m_device;
+};
+
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/PCMSim.h b/wpilibc/src/main/native/include/frc/simulation/PCMSim.h
new file mode 100644
index 0000000..2e3ed5e
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/PCMSim.h
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class Compressor;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Pneumatic Control Module (PCM).
+ */
+class PCMSim {
+ public:
+  /**
+   * Constructs with the default PCM module number (CAN ID).
+   */
+  PCMSim();
+
+  /**
+   * Constructs from a PCM module number (CAN ID).
+   *
+   * @param module module number
+   */
+  explicit PCMSim(int module);
+
+  /**
+   * Constructs from a Compressor object.
+   *
+   * @param compressor Compressor connected to PCM to simulate
+   */
+  explicit PCMSim(const Compressor& compressor);
+
+  std::unique_ptr<CallbackStore> RegisterSolenoidInitializedCallback(
+      int channel, NotifyCallback callback, bool initialNotify);
+
+  bool GetSolenoidInitialized(int channel) const;
+
+  void SetSolenoidInitialized(int channel, bool solenoidInitialized);
+
+  std::unique_ptr<CallbackStore> RegisterSolenoidOutputCallback(
+      int channel, NotifyCallback callback, bool initialNotify);
+
+  bool GetSolenoidOutput(int channel) const;
+
+  void SetSolenoidOutput(int channel, bool solenoidOutput);
+
+  std::unique_ptr<CallbackStore> RegisterCompressorInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetCompressorInitialized() const;
+
+  void SetCompressorInitialized(bool compressorInitialized);
+
+  std::unique_ptr<CallbackStore> RegisterCompressorOnCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetCompressorOn() const;
+
+  void SetCompressorOn(bool compressorOn);
+
+  std::unique_ptr<CallbackStore> RegisterClosedLoopEnabledCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetClosedLoopEnabled() const;
+
+  void SetClosedLoopEnabled(bool closedLoopEnabled);
+
+  std::unique_ptr<CallbackStore> RegisterPressureSwitchCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetPressureSwitch() const;
+
+  void SetPressureSwitch(bool pressureSwitch);
+
+  std::unique_ptr<CallbackStore> RegisterCompressorCurrentCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetCompressorCurrent() const;
+
+  void SetCompressorCurrent(double compressorCurrent);
+
+  uint8_t GetAllSolenoidOutputs() const;
+
+  void SetAllSolenoidOutputs(uint8_t outputs);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/PDPSim.h b/wpilibc/src/main/native/include/frc/simulation/PDPSim.h
new file mode 100644
index 0000000..c67815f
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/PDPSim.h
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class PowerDistributionPanel;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Power Distribution Panel (PDP).
+ */
+class PDPSim {
+ public:
+  /**
+   * Constructs from a PDP module number (CAN ID).
+   *
+   * @param module module number
+   */
+  explicit PDPSim(int module = 0);
+
+  /**
+   * Constructs from a PowerDistributionPanel object.
+   *
+   * @param pdp PowerDistributionPanel to simulate
+   */
+  explicit PDPSim(const PowerDistributionPanel& pdp);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterTemperatureCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetTemperature() const;
+
+  void SetTemperature(double temperature);
+
+  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetVoltage() const;
+
+  void SetVoltage(double voltage);
+
+  std::unique_ptr<CallbackStore> RegisterCurrentCallback(
+      int channel, NotifyCallback callback, bool initialNotify);
+
+  double GetCurrent(int channel) const;
+
+  void SetCurrent(int channel, double current);
+
+  void GetAllCurrents(double* currents) const;
+
+  void SetAllCurrents(const double* currents);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/PWMSim.h b/wpilibc/src/main/native/include/frc/simulation/PWMSim.h
new file mode 100644
index 0000000..ca2dfca
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/PWMSim.h
@@ -0,0 +1,87 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class PWM;
+
+namespace sim {
+
+/**
+ * Class to control a simulated PWM output.
+ */
+class PWMSim {
+ public:
+  /**
+   * Constructs from a PWM object.
+   *
+   * @param pwm PWM to simulate
+   */
+  explicit PWMSim(const PWM& pwm);
+
+  /**
+   * Constructs from a PWM channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit PWMSim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitialized() const;
+
+  void SetInitialized(bool initialized);
+
+  std::unique_ptr<CallbackStore> RegisterRawValueCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetRawValue() const;
+
+  void SetRawValue(int rawValue);
+
+  std::unique_ptr<CallbackStore> RegisterSpeedCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  double GetSpeed() const;
+
+  void SetSpeed(double speed);
+
+  std::unique_ptr<CallbackStore> RegisterPositionCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  double GetPosition() const;
+
+  void SetPosition(double position);
+
+  std::unique_ptr<CallbackStore> RegisterPeriodScaleCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  int GetPeriodScale() const;
+
+  void SetPeriodScale(int periodScale);
+
+  std::unique_ptr<CallbackStore> RegisterZeroLatchCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetZeroLatch() const;
+
+  void SetZeroLatch(bool zeroLatch);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/RelaySim.h b/wpilibc/src/main/native/include/frc/simulation/RelaySim.h
new file mode 100644
index 0000000..99a8eef
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/RelaySim.h
@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+
+class Relay;
+
+namespace sim {
+
+/**
+ * Class to control a simulated relay.
+ */
+class RelaySim {
+ public:
+  /**
+   * Constructs from a Relay object.
+   *
+   * @param relay Relay to simulate
+   */
+  explicit RelaySim(const Relay& relay);
+
+  /**
+   * Constructs from a relay channel number.
+   *
+   * @param channel Channel number
+   */
+  explicit RelaySim(int channel);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedForwardCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitializedForward() const;
+
+  void SetInitializedForward(bool initializedForward);
+
+  std::unique_ptr<CallbackStore> RegisterInitializedReverseCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetInitializedReverse() const;
+
+  void SetInitializedReverse(bool initializedReverse);
+
+  std::unique_ptr<CallbackStore> RegisterForwardCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetForward() const;
+
+  void SetForward(bool forward);
+
+  std::unique_ptr<CallbackStore> RegisterReverseCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  bool GetReverse() const;
+
+  void SetReverse(bool reverse);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h b/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h
new file mode 100644
index 0000000..32eb462
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h
@@ -0,0 +1,133 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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 <units/current.h>
+#include <units/voltage.h>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+namespace sim {
+
+/**
+ * Class to control a simulated RoboRIO.
+ */
+class RoboRioSim {
+ public:
+  static std::unique_ptr<CallbackStore> RegisterFPGAButtonCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetFPGAButton();
+
+  static void SetFPGAButton(bool fPGAButton);
+
+  static std::unique_ptr<CallbackStore> RegisterVInVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::volt_t GetVInVoltage();
+
+  static void SetVInVoltage(units::volt_t vInVoltage);
+
+  static std::unique_ptr<CallbackStore> RegisterVInCurrentCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::ampere_t GetVInCurrent();
+
+  static void SetVInCurrent(units::ampere_t vInCurrent);
+
+  static std::unique_ptr<CallbackStore> RegisterUserVoltage6VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::volt_t GetUserVoltage6V();
+
+  static void SetUserVoltage6V(units::volt_t userVoltage6V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserCurrent6VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::ampere_t GetUserCurrent6V();
+
+  static void SetUserCurrent6V(units::ampere_t userCurrent6V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserActive6VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetUserActive6V();
+
+  static void SetUserActive6V(bool userActive6V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserVoltage5VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::volt_t GetUserVoltage5V();
+
+  static void SetUserVoltage5V(units::volt_t userVoltage5V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserCurrent5VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::ampere_t GetUserCurrent5V();
+
+  static void SetUserCurrent5V(units::ampere_t userCurrent5V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserActive5VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetUserActive5V();
+
+  static void SetUserActive5V(bool userActive5V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserVoltage3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::volt_t GetUserVoltage3V3();
+
+  static void SetUserVoltage3V3(units::volt_t userVoltage3V3);
+
+  static std::unique_ptr<CallbackStore> RegisterUserCurrent3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  static units::ampere_t GetUserCurrent3V3();
+
+  static void SetUserCurrent3V3(units::ampere_t userCurrent3V3);
+
+  static std::unique_ptr<CallbackStore> RegisterUserActive3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  static bool GetUserActive3V3();
+
+  static void SetUserActive3V3(bool userActive3V3);
+
+  static std::unique_ptr<CallbackStore> RegisterUserFaults6VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static int GetUserFaults6V();
+
+  static void SetUserFaults6V(int userFaults6V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserFaults5VCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  static int GetUserFaults5V();
+
+  static void SetUserFaults5V(int userFaults5V);
+
+  static std::unique_ptr<CallbackStore> RegisterUserFaults3V3Callback(
+      NotifyCallback callback, bool initialNotify);
+
+  static int GetUserFaults3V3();
+
+  static void SetUserFaults3V3(int userFaults3V3);
+
+  static void ResetData();
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h b/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h
new file mode 100644
index 0000000..f2e3249
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/CallbackStore.h"
+
+namespace frc {
+namespace sim {
+class SPIAccelerometerSim {
+ public:
+  explicit SPIAccelerometerSim(int index);
+
+  std::unique_ptr<CallbackStore> RegisterActiveCallback(NotifyCallback callback,
+                                                        bool initialNotify);
+
+  bool GetActive() const;
+
+  void SetActive(bool active);
+
+  std::unique_ptr<CallbackStore> RegisterRangeCallback(NotifyCallback callback,
+                                                       bool initialNotify);
+
+  int GetRange() const;
+
+  void SetRange(int range);
+
+  std::unique_ptr<CallbackStore> RegisterXCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetX() const;
+
+  void SetX(double x);
+
+  std::unique_ptr<CallbackStore> RegisterYCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetY() const;
+
+  void SetY(double y);
+
+  std::unique_ptr<CallbackStore> RegisterZCallback(NotifyCallback callback,
+                                                   bool initialNotify);
+
+  double GetZ() const;
+
+  void SetZ(double z);
+
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h b/wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h
new file mode 100644
index 0000000..640cc0e
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <string>
+#include <vector>
+
+#include <hal/SimDevice.h>
+#include <hal/simulation/SimDeviceData.h>
+
+namespace frc {
+namespace sim {
+
+/**
+ * Class to control the simulation side of a SimDevice.
+ */
+class SimDeviceSim {
+ public:
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   */
+  explicit SimDeviceSim(const char* name);
+
+  hal::SimValue GetValue(const char* name) const;
+
+  hal::SimDouble GetDouble(const char* name) const;
+
+  hal::SimEnum GetEnum(const char* name) const;
+
+  hal::SimBoolean GetBoolean(const char* name) const;
+
+  static std::vector<std::string> GetEnumOptions(hal::SimEnum val);
+
+  template <typename F>
+  void EnumerateValues(F callback) const {
+    return HALSIM_EnumerateSimValues(
+        m_handle, &callback,
+        [](const char* name, void* param, HAL_SimValueHandle handle,
+           HAL_Bool readonly, const struct HAL_Value* value) {
+          std::invoke(*static_cast<F*>(param), name, handle, readonly, value);
+        });
+  }
+
+  operator HAL_SimDeviceHandle() const { return m_handle; }
+
+  template <typename F>
+  static void EnumerateDevices(const char* prefix, F callback) {
+    return HALSIM_EnumerateSimDevices(
+        prefix, &callback,
+        [](const char* name, void* param, HAL_SimDeviceHandle handle) {
+          std::invoke(*static_cast<F*>(param), name, handle);
+        });
+  }
+
+  static void ResetData();
+
+ private:
+  HAL_SimDeviceHandle m_handle;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/SimHooks.h b/wpilibc/src/main/native/include/frc/simulation/SimHooks.h
new file mode 100644
index 0000000..7690a4f
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/SimHooks.h
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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 <stdint.h>
+
+#include <hal/HALBase.h>
+#include <units/time.h>
+
+namespace frc {
+namespace sim {
+
+void SetRuntimeType(HAL_RuntimeType type);
+
+void WaitForProgramStart();
+
+void SetProgramStarted();
+
+bool GetProgramStarted();
+
+void RestartTiming();
+
+void PauseTiming();
+
+void ResumeTiming();
+
+bool IsTimingPaused();
+
+void StepTiming(units::second_t delta);
+
+void StepTimingAsync(units::second_t delta);
+
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
new file mode 100644
index 0000000..f8de822
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
@@ -0,0 +1,147 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <array>
+
+#include <units/angle.h>
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/moment_of_inertia.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated arm mechanism.
+ */
+class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
+ public:
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param system             The system representing this arm.
+   * @param gearbox            The type and number of motors on the arm gearbox.
+   * @param gearing            The gear ratio of the arm (numbers greater than 1
+   *                           represent reductions).
+   * @param armLength          The length of the arm.
+   * @param minAngle           The minimum angle that the arm is capable of.
+   * @param maxAngle           The maximum angle that the arm is capable of.
+   * @param mass               The mass of the arm.
+   * @param measurementStdDevs The standard deviations of the measurements.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   */
+  SingleJointedArmSim(const LinearSystem<2, 1, 1>& system,
+                      const DCMotor& gearbox, double gearing,
+                      units::meter_t armLength, units::radian_t minAngle,
+                      units::radian_t maxAngle, units::kilogram_t mass,
+                      bool simulateGravity,
+                      const std::array<double, 1>& measurementStdDevs = {0.0});
+  /**
+   * Creates a simulated arm mechanism.
+   *
+   * @param gearbox            The type and number of motors on the arm gearbox.
+   * @param gearing            The gear ratio of the arm (numbers greater than 1
+   *                           represent reductions).
+   * @param moi                The moment of inertia of the arm. This can be
+   *                           calculated from CAD software.
+   * @param armLength          The length of the arm.
+   * @param minAngle           The minimum angle that the arm is capable of.
+   * @param maxAngle           The maximum angle that the arm is capable of.
+   * @param mass               The mass of the arm.
+   * @param measurementStdDevs The standard deviation of the measurement noise.
+   * @param simulateGravity    Whether gravity should be simulated or not.
+   */
+  SingleJointedArmSim(const DCMotor& gearbox, double gearing,
+                      units::kilogram_square_meter_t moi,
+                      units::meter_t armLength, units::radian_t minAngle,
+                      units::radian_t maxAngle, units::kilogram_t mass,
+                      bool simulateGravity,
+                      const std::array<double, 1>& measurementStdDevs = {0.0});
+
+  /**
+   * Returns whether the arm has hit the lower limit.
+   *
+   * @param x The current arm state.
+   * @return Whether the arm has hit the lower limit.
+   */
+  bool HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const;
+
+  /**
+   * Returns whether the arm has hit the upper limit.
+   *
+   * @param x The current arm state.
+   * @return Whether the arm has hit the upper limit.
+   */
+  bool HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const;
+
+  /**
+   * Returns the current arm angle.
+   *
+   * @return The current arm angle.
+   */
+  units::radian_t GetAngle() const;
+
+  /**
+   * Returns the current arm velocity.
+   *
+   * @return The current arm velocity.
+   */
+  units::radians_per_second_t GetVelocity() const;
+
+  /**
+   * Returns the arm current draw.
+   *
+   * @return The arm current draw.
+   */
+  units::ampere_t GetCurrentDraw() const override;
+
+  /**
+   * Sets the input voltage for the elevator.
+   *
+   * @param voltage The input voltage.
+   */
+  void SetInputVoltage(units::volt_t voltage);
+
+  /**
+   * Calculates a rough estimate of the moment of inertia of an arm given its
+   * length and mass.
+   *
+   * @param length The length of the arm.
+   * @param mass   The mass of the arm.
+   *
+   * @return The calculated moment of inertia.
+   */
+  static constexpr units::kilogram_square_meter_t EstimateMOI(
+      units::meter_t length, units::kilogram_t mass) {
+    return 1.0 / 3.0 * mass * length * length;
+  }
+
+ protected:
+  /**
+   * Updates the state estimate of the arm.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (voltage).
+   * @param dt          The time difference between controller updates.
+   */
+  Eigen::Matrix<double, 2, 1> UpdateX(
+      const Eigen::Matrix<double, 2, 1>& currentXhat,
+      const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
+
+ private:
+  units::meter_t m_r;
+  units::radian_t m_minAngle;
+  units::radian_t m_maxAngle;
+  units::kilogram_t m_mass;
+  const DCMotor m_gearbox;
+  double m_gearing;
+  bool m_simulateGravity;
+};
+}  // namespace frc::sim
diff --git a/wpilibc/src/main/native/include/frc/simulation/XboxControllerSim.h b/wpilibc/src/main/native/include/frc/simulation/XboxControllerSim.h
new file mode 100644
index 0000000..d981111
--- /dev/null
+++ b/wpilibc/src/main/native/include/frc/simulation/XboxControllerSim.h
@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/simulation/GenericHIDSim.h"
+
+namespace frc {
+
+class XboxController;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Xbox 360 or Xbox One controller.
+ */
+class XboxControllerSim : public GenericHIDSim {
+ public:
+  /**
+   * Constructs from a XboxController object.
+   *
+   * @param joystick controller to simulate
+   */
+  explicit XboxControllerSim(const XboxController& joystick);
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit XboxControllerSim(int port);
+
+  void SetX(GenericHID::JoystickHand hand, double value);
+
+  void SetY(GenericHID::JoystickHand hand, double value);
+
+  void SetTriggerAxis(GenericHID::JoystickHand hand, double value);
+
+  void SetBumper(GenericHID::JoystickHand hand, bool state);
+
+  void SetStickButton(GenericHID::JoystickHand hand, bool state);
+
+  void SetAButton(bool state);
+
+  void SetBButton(bool state);
+
+  void SetXButton(bool state);
+
+  void SetYButton(bool state);
+
+  void SetBackButton(bool state);
+
+  void SetStartButton(bool state);
+};
+
+}  // namespace sim
+}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h
index a0ea0f9..98419b9 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 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.                                                               */
@@ -17,7 +17,9 @@
 class SendableBase : public Sendable, public SendableHelper<SendableBase> {
  public:
   /**
-   * Creates an instance of the sensor base.
+   * Creates an instance of the sensor base
+   *
+   * @deprecated use Sendable and SendableHelper
    *
    * @param addLiveWindow if true, add this Sendable to LiveWindow
    */
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
index eb69dcd..e5dea44 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2017-2020 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.                                                               */
@@ -202,7 +202,7 @@
 
   std::vector<Property> m_properties;
   std::function<void()> m_safeState;
-  std::function<void()> m_updateTable;
+  std::vector<std::function<void()>> m_updateTables;
   std::shared_ptr<nt::NetworkTable> m_table;
   nt::NetworkTableEntry m_controllableEntry;
   bool m_actuator = false;
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h
index 6cf3c47..b370fc0 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 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.                                                               */
@@ -46,13 +46,38 @@
 
  public:
   ~SendableChooser() override = default;
+  SendableChooser() = default;
+  SendableChooser(SendableChooser&& rhs) = default;
+  SendableChooser& operator=(SendableChooser&& rhs) = default;
 
   void AddOption(wpi::StringRef name, T object);
   void SetDefaultOption(wpi::StringRef name, T object);
 
+  /**
+   * Adds the given object to the list of options.
+   *
+   * On the SmartDashboard on the desktop, the object will appear as the given
+   * name.
+   *
+   * @deprecated use AddOption(wpi::StringRef name, T object) instead.
+   *
+   * @param name   the name of the option
+   * @param object the option
+   */
   WPI_DEPRECATED("use AddOption() instead")
   void AddObject(wpi::StringRef name, T object) { AddOption(name, object); }
 
+  /**
+   * Add the given object to the list of options and marks it as the default.
+   *
+   * Functionally, this is very close to AddOption() except that it will use
+   * this as the default option if none other is explicitly selected.
+   *
+   * @deprecated use SetDefaultOption(wpi::StringRef name, T object) instead.
+   *
+   * @param name   the name of the option
+   * @param object the option
+   */
   WPI_DEPRECATED("use SetDefaultOption() instead")
   void AddDefault(wpi::StringRef name, T object) {
     SetDefaultOption(name, object);
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
index 295d263..57e2828 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2011-2020 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.                                                               */
@@ -76,20 +76,21 @@
 void SendableChooser<T>::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("String Chooser");
   builder.GetEntry(kInstance).SetDouble(m_instance);
-  builder.AddStringArrayProperty(kOptions,
-                                 [=]() {
-                                   std::vector<std::string> keys;
-                                   for (const auto& choice : m_choices) {
-                                     keys.push_back(choice.first());
-                                   }
+  builder.AddStringArrayProperty(
+      kOptions,
+      [=]() {
+        std::vector<std::string> keys;
+        for (const auto& choice : m_choices) {
+          keys.push_back(choice.first());
+        }
 
-                                   // Unlike std::map, wpi::StringMap elements
-                                   // are not sorted
-                                   std::sort(keys.begin(), keys.end());
+        // Unlike std::map, wpi::StringMap elements
+        // are not sorted
+        std::sort(keys.begin(), keys.end());
 
-                                   return keys;
-                                 },
-                                 nullptr);
+        return keys;
+      },
+      nullptr);
   builder.AddSmallStringProperty(
       kDefault,
       [=](wpi::SmallVectorImpl<char>&) -> wpi::StringRef {
diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h b/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h
index 6b0b28b..1f39216 100644
--- a/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h
+++ b/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -46,6 +46,8 @@
   /**
    * Gets the name of this Sendable object.
    *
+   * @deprecated use SendableRegistry::GetName()
+   *
    * @return Name
    */
   WPI_DEPRECATED("use SendableRegistry::GetName()")
@@ -57,6 +59,8 @@
   /**
    * Sets the name of this Sendable object.
    *
+   * @deprecated use SendableRegistry::SetName()
+   *
    * @param name name
    */
   WPI_DEPRECATED("use SendableRegistry::SetName()")
@@ -67,6 +71,8 @@
   /**
    * Sets both the subsystem name and device name of this Sendable object.
    *
+   * @deprecated use SendableRegistry::SetName()
+   *
    * @param subsystem subsystem name
    * @param name device name
    */
@@ -79,6 +85,8 @@
   /**
    * Gets the subsystem name of this Sendable object.
    *
+   * @deprecated use SendableRegistry::GetSubsystem().
+   *
    * @return Subsystem name
    */
   WPI_DEPRECATED("use SendableRegistry::GetSubsystem()")
@@ -90,6 +98,8 @@
   /**
    * Sets the subsystem name of this Sendable object.
    *
+   * @deprecated use SendableRegistry::SetSubsystem()
+   *
    * @param subsystem subsystem name
    */
   WPI_DEPRECATED("use SendableRegistry::SetSubsystem()")
@@ -102,6 +112,8 @@
   /**
    * Add a child component.
    *
+   * @deprecated use SendableRegistry::AddChild()
+   *
    * @param child child component
    */
   WPI_DEPRECATED("use SendableRegistry::AddChild()")
@@ -113,6 +125,8 @@
   /**
    * Add a child component.
    *
+   * @deprecated use SendableRegistry::AddChild()
+   *
    * @param child child component
    */
   WPI_DEPRECATED("use SendableRegistry::AddChild()")
@@ -124,6 +138,8 @@
   /**
    * Sets the name of the sensor with a channel number.
    *
+   * @deprecated use SendableRegistry::SetName()
+   *
    * @param moduleType A string that defines the module name in the label for
    *                   the value
    * @param channel    The channel number the device is plugged into
@@ -137,6 +153,8 @@
   /**
    * Sets the name of the sensor with a module and channel number.
    *
+   * @deprecated use SendableRegistry::SetName()
+   *
    * @param moduleType   A string that defines the module name in the label for
    *                     the value
    * @param moduleNumber The number of the particular module type
diff --git a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
deleted file mode 100644
index 1e08a94..0000000
--- a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ /dev/null
@@ -1,85 +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 <array>
-
-#include <Eigen/Core>
-
-#include "frc/spline/Spline.h"
-
-namespace frc {
-/**
- * Represents a hermite spline of degree 3.
- */
-class CubicHermiteSpline : public Spline<3> {
- public:
-  /**
-   * Constructs a cubic hermite spline with the specified control vectors. Each
-   * control vector contains info about the location of the point and its first
-   * derivative.
-   *
-   * @param xInitialControlVector The control vector for the initial point in
-   * the x dimension.
-   * @param xFinalControlVector The control vector for the final point in
-   * the x dimension.
-   * @param yInitialControlVector The control vector for the initial point in
-   * the y dimension.
-   * @param yFinalControlVector The control vector for the final point in
-   * the y dimension.
-   */
-  CubicHermiteSpline(std::array<double, 2> xInitialControlVector,
-                     std::array<double, 2> xFinalControlVector,
-                     std::array<double, 2> yInitialControlVector,
-                     std::array<double, 2> yFinalControlVector);
-
- protected:
-  /**
-   * Returns the coefficients matrix.
-   * @return The coefficients matrix.
-   */
-  Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
-    return m_coefficients;
-  }
-
- private:
-  Eigen::Matrix<double, 6, 4> m_coefficients =
-      Eigen::Matrix<double, 6, 4>::Zero();
-
-  /**
-   * Returns the hermite basis matrix for cubic hermite spline interpolation.
-   * @return The hermite basis matrix for cubic hermite spline interpolation.
-   */
-  static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
-    // clang-format off
-    static auto basis = (Eigen::Matrix<double, 4, 4>() <<
-     +2.0, +1.0, -2.0, +1.0,
-     -3.0, -2.0, +3.0, -1.0,
-     +0.0, +1.0, +0.0, +0.0,
-     +1.0, +0.0, +0.0, +0.0).finished();
-    // clang-format on
-    return basis;
-  }
-
-  /**
-   * Returns the control vector for each dimension as a matrix from the
-   * user-provided arrays in the constructor.
-   *
-   * @param initialVector The control vector for the initial point.
-   * @param finalVector The control vector for the final point.
-   *
-   * @return The control vector matrix for a dimension.
-   */
-  static Eigen::Vector4d ControlVectorFromArrays(
-      std::array<double, 2> initialVector, std::array<double, 2> finalVector) {
-    return (Eigen::Vector4d() << initialVector[0], initialVector[1],
-            finalVector[0], finalVector[1])
-        .finished();
-  }
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
deleted file mode 100644
index 632d3ad..0000000
--- a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h
+++ /dev/null
@@ -1,87 +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 <array>
-
-#include <Eigen/Core>
-
-#include "frc/spline/Spline.h"
-
-namespace frc {
-/**
- * Represents a hermite spline of degree 5.
- */
-class QuinticHermiteSpline : public Spline<5> {
- public:
-  /**
-   * Constructs a quintic hermite spline with the specified control vectors.
-   * Each control vector contains into about the location of the point, its
-   * first derivative, and its second derivative.
-   *
-   * @param xInitialControlVector The control vector for the initial point in
-   * the x dimension.
-   * @param xFinalControlVector The control vector for the final point in
-   * the x dimension.
-   * @param yInitialControlVector The control vector for the initial point in
-   * the y dimension.
-   * @param yFinalControlVector The control vector for the final point in
-   * the y dimension.
-   */
-  QuinticHermiteSpline(std::array<double, 3> xInitialControlVector,
-                       std::array<double, 3> xFinalControlVector,
-                       std::array<double, 3> yInitialControlVector,
-                       std::array<double, 3> yFinalControlVector);
-
- protected:
-  /**
-   * Returns the coefficients matrix.
-   * @return The coefficients matrix.
-   */
-  Eigen::Matrix<double, 6, 6> Coefficients() const override {
-    return m_coefficients;
-  }
-
- private:
-  Eigen::Matrix<double, 6, 6> m_coefficients =
-      Eigen::Matrix<double, 6, 6>::Zero();
-
-  /**
-   * Returns the hermite basis matrix for quintic hermite spline interpolation.
-   * @return The hermite basis matrix for quintic hermite spline interpolation.
-   */
-  static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
-    // clang-format off
-    static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
-      -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
-      +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
-      -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
-      +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
-      +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
-      +01.0, +00.0, +00.0, +00.0, +00.0, +00.0).finished();
-    // clang-format on
-    return basis;
-  }
-
-  /**
-   * Returns the control vector for each dimension as a matrix from the
-   * user-provided arrays in the constructor.
-   *
-   * @param initialVector The control vector for the initial point.
-   * @param finalVector The control vector for the final point.
-   *
-   * @return The control vector matrix for a dimension.
-   */
-  static Eigen::Matrix<double, 6, 1> ControlVectorFromArrays(
-      std::array<double, 3> initialVector, std::array<double, 3> finalVector) {
-    return (Eigen::Matrix<double, 6, 1>() << initialVector[0], initialVector[1],
-            initialVector[2], finalVector[0], finalVector[1], finalVector[2])
-        .finished();
-  }
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/spline/Spline.h b/wpilibc/src/main/native/include/frc/spline/Spline.h
deleted file mode 100644
index e8f2372..0000000
--- a/wpilibc/src/main/native/include/frc/spline/Spline.h
+++ /dev/null
@@ -1,135 +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 <array>
-#include <utility>
-#include <vector>
-
-#include <Eigen/Core>
-
-#include "frc/geometry/Pose2d.h"
-
-namespace frc {
-
-/**
- * Define a unit for curvature.
- */
-using curvature_t = units::unit_t<
-    units::compound_unit<units::radian, units::inverse<units::meter>>>;
-
-/**
- * Represents a two-dimensional parametric spline that interpolates between two
- * points.
- *
- * @tparam Degree The degree of the spline.
- */
-template <int Degree>
-class Spline {
- public:
-  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
-
-  Spline() = default;
-
-  Spline(const Spline&) = default;
-  Spline& operator=(const Spline&) = default;
-
-  Spline(Spline&&) = default;
-  Spline& operator=(Spline&&) = default;
-
-  virtual ~Spline() = default;
-
-  /**
-   * Represents a control vector for a spline.
-   *
-   * Each element in each array represents the value of the derivative at the
-   * index. For example, the value of x[2] is the second derivative in the x
-   * dimension.
-   */
-  struct ControlVector {
-    std::array<double, (Degree + 1) / 2> x;
-    std::array<double, (Degree + 1) / 2> y;
-  };
-
-  /**
-   * Gets the pose and curvature at some point t on the spline.
-   *
-   * @param t The point t
-   * @return The pose and curvature at that point.
-   */
-  PoseWithCurvature GetPoint(double t) const {
-    Eigen::Matrix<double, Degree + 1, 1> polynomialBases;
-
-    // Populate the polynomial bases
-    for (int i = 0; i <= Degree; i++) {
-      polynomialBases(i) = std::pow(t, Degree - i);
-    }
-
-    // This simply multiplies by the coefficients. We need to divide out t some
-    // n number of times where n is the derivative we want to take.
-    Eigen::Matrix<double, 6, 1> combined = Coefficients() * polynomialBases;
-
-    double dx, dy, ddx, ddy;
-
-    // If t = 0, all other terms in the equation cancel out to zero. We can use
-    // the last x^0 term in the equation.
-    if (t == 0.0) {
-      dx = Coefficients()(2, Degree - 1);
-      dy = Coefficients()(3, Degree - 1);
-      ddx = Coefficients()(4, Degree - 2);
-      ddy = Coefficients()(5, Degree - 2);
-    } else {
-      // Divide out t for first derivative.
-      dx = combined(2) / t;
-      dy = combined(3) / t;
-
-      // Divide out t for second derivative.
-      ddx = combined(4) / t / t;
-      ddy = combined(5) / t / t;
-    }
-
-    // Find the curvature.
-    const auto curvature =
-        (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
-
-    return {
-        {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d(dx, dy)},
-        curvature_t(curvature)};
-  }
-
- protected:
-  /**
-   * Returns the coefficients of the spline.
-   *
-   * @return The coefficients of the spline.
-   */
-  virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
-
-  /**
-   * Converts a Translation2d into a vector that is compatible with Eigen.
-   *
-   * @param translation The Translation2d to convert.
-   * @return The vector.
-   */
-  static Eigen::Vector2d ToVector(const Translation2d& translation) {
-    return (Eigen::Vector2d() << translation.X().to<double>(),
-            translation.Y().to<double>())
-        .finished();
-  }
-
-  /**
-   * Converts an Eigen vector into a Translation2d.
-   *
-   * @param vector The vector to convert.
-   * @return The Translation2d.
-   */
-  static Translation2d FromVector(const Eigen::Vector2d& vector) {
-    return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1)));
-  }
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/spline/SplineHelper.h b/wpilibc/src/main/native/include/frc/spline/SplineHelper.h
deleted file mode 100644
index 4ed1cf5..0000000
--- a/wpilibc/src/main/native/include/frc/spline/SplineHelper.h
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <array>
-#include <utility>
-#include <vector>
-
-#include "frc/spline/CubicHermiteSpline.h"
-#include "frc/spline/QuinticHermiteSpline.h"
-
-namespace frc {
-/**
- * Helper class that is used to generate cubic and quintic splines from user
- * provided waypoints.
- */
-class SplineHelper {
- public:
-  /**
-   * Returns 2 cubic control vectors from a set of exterior waypoints and
-   * interior translations.
-   *
-   * @param start             The starting pose.
-   * @param interiorWaypoints The interior waypoints.
-   * @param end               The ending pose.
-   * @return 2 cubic control vectors.
-   */
-  static std::array<Spline<3>::ControlVector, 2>
-  CubicControlVectorsFromWaypoints(
-      const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
-      const Pose2d& end);
-
-  /**
-   * Returns quintic control vectors from a set of waypoints.
-   *
-   * @param waypoints The waypoints
-   * @return List of control vectors
-   */
-  static std::vector<Spline<5>::ControlVector>
-  QuinticControlVectorsFromWaypoints(const std::vector<Pose2d>& waypoints);
-
-  /**
-   * Returns a set of cubic splines corresponding to the provided control
-   * vectors. The user is free to set the direction of the start and end
-   * point. The directions for the middle waypoints are determined
-   * automatically to ensure continuous curvature throughout the path.
-   *
-   * The derivation for the algorithm used can be found here:
-   * <https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08/undervisningsmateriale/chap7alecture.pdf>
-   *
-   * @param start The starting control vector.
-   * @param waypoints The middle waypoints. This can be left blank if you
-   * only wish to create a path with two waypoints.
-   * @param end The ending control vector.
-   *
-   * @return A vector of cubic hermite splines that interpolate through the
-   * provided waypoints.
-   */
-  static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
-      const Spline<3>::ControlVector& start,
-      std::vector<Translation2d> waypoints,
-      const Spline<3>::ControlVector& end);
-
-  /**
-   * Returns a set of quintic splines corresponding to the provided control
-   * vectors. The user is free to set the direction of all waypoints. Continuous
-   * curvature is guaranteed throughout the path.
-   *
-   * @param controlVectors The control vectors.
-   * @return A vector of quintic hermite splines that interpolate through the
-   * provided waypoints.
-   */
-  static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
-      const std::vector<Spline<5>::ControlVector>& controlVectors);
-
- private:
-  static Spline<3>::ControlVector CubicControlVector(double scalar,
-                                                     const Pose2d& point) {
-    return {
-        {point.Translation().X().to<double>(), scalar * point.Rotation().Cos()},
-        {point.Translation().Y().to<double>(),
-         scalar * point.Rotation().Sin()}};
-  }
-
-  static Spline<5>::ControlVector QuinticControlVector(double scalar,
-                                                       const Pose2d& point) {
-    return {{point.Translation().X().to<double>(),
-             scalar * point.Rotation().Cos(), 0.0},
-            {point.Translation().Y().to<double>(),
-             scalar * point.Rotation().Sin(), 0.0}};
-  }
-
-  /**
-   * Thomas algorithm for solving tridiagonal systems Af = d.
-   *
-   * @param a the values of A above the diagonal
-   * @param b the values of A on the diagonal
-   * @param c the values of A below the diagonal
-   * @param d the vector on the rhs
-   * @param solutionVector the unknown (solution) vector, modified in-place
-   */
-  static void ThomasAlgorithm(const std::vector<double>& a,
-                              const std::vector<double>& b,
-                              const std::vector<double>& c,
-                              const std::vector<double>& d,
-                              std::vector<double>* solutionVector);
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h b/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
deleted file mode 100644
index d3a5e6e..0000000
--- a/wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h
+++ /dev/null
@@ -1,142 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-/*
- * MIT License
- *
- * Copyright (c) 2018 Team 254
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#pragma once
-
-#include <frc/spline/Spline.h>
-
-#include <stack>
-#include <string>
-#include <utility>
-#include <vector>
-
-#include <units/units.h>
-#include <wpi/Twine.h>
-
-namespace frc {
-
-/**
- * Class used to parameterize a spline by its arc length.
- */
-class SplineParameterizer {
- public:
-  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
-
-  struct MalformedSplineException : public std::runtime_error {
-    explicit MalformedSplineException(const char* what_arg)
-        : runtime_error(what_arg) {}
-  };
-
-  /**
-   * Parameterizes the spline. This method breaks up the spline into various
-   * arcs until their dx, dy, and dtheta are within specific tolerances.
-   *
-   * @param spline The spline to parameterize.
-   * @param t0 Starting internal spline parameter. It is recommended to leave
-   * this as default.
-   * @param t1 Ending internal spline parameter. It is recommended to leave this
-   * as default.
-   *
-   * @return A vector of poses and curvatures that represents various points on
-   * the spline.
-   */
-  template <int Dim>
-  static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
-                                                     double t0 = 0.0,
-                                                     double t1 = 1.0) {
-    std::vector<PoseWithCurvature> splinePoints;
-
-    // The parameterization does not add the initial point. Let's add that.
-    splinePoints.push_back(spline.GetPoint(t0));
-
-    // We use an "explicit stack" to simulate recursion, instead of a recursive
-    // function call This give us greater control, instead of a stack overflow
-    std::stack<StackContents> stack;
-    stack.emplace(StackContents{t0, t1});
-
-    StackContents current;
-    PoseWithCurvature start;
-    PoseWithCurvature end;
-    int iterations = 0;
-
-    while (!stack.empty()) {
-      current = stack.top();
-      stack.pop();
-      start = spline.GetPoint(current.t0);
-      end = spline.GetPoint(current.t1);
-
-      const auto twist = start.first.Log(end.first);
-
-      if (units::math::abs(twist.dy) > kMaxDy ||
-          units::math::abs(twist.dx) > kMaxDx ||
-          units::math::abs(twist.dtheta) > kMaxDtheta) {
-        stack.emplace(StackContents{(current.t0 + current.t1) / 2, current.t1});
-        stack.emplace(StackContents{current.t0, (current.t0 + current.t1) / 2});
-      } else {
-        splinePoints.push_back(spline.GetPoint(current.t1));
-      }
-
-      if (iterations++ >= kMaxIterations) {
-        throw MalformedSplineException(
-            "Could not parameterize a malformed spline. "
-            "This means that you probably had two or more adjacent "
-            "waypoints that were very close together with headings "
-            "in opposing directions.");
-      }
-    }
-
-    return splinePoints;
-  }
-
- private:
-  // Constraints for spline parameterization.
-  static constexpr units::meter_t kMaxDx = 5_in;
-  static constexpr units::meter_t kMaxDy = 0.05_in;
-  static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
-
-  struct StackContents {
-    double t0;
-    double t1;
-  };
-
-  /**
-   * A malformed spline does not actually explode the LIFO stack size. Instead,
-   * the stack size stays at a relatively small number (e.g. 30) and never
-   * decreases. Because of this, we must count iterations. Even long, complex
-   * paths don't usually go over 300 iterations, so hitting this maximum should
-   * definitely indicate something has gone wrong.
-   */
-  static constexpr int kMaxIterations = 5000;
-
-  friend class CubicHermiteSplineTest;
-  friend class QuinticHermiteSplineTest;
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h b/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
deleted file mode 100644
index 13e32d6..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/Trajectory.h
+++ /dev/null
@@ -1,160 +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 <vector>
-
-#include <units/units.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/geometry/Transform2d.h"
-
-namespace wpi {
-class json;
-}  // namespace wpi
-
-namespace frc {
-
-/**
- * Define a unit for curvature.
- */
-using curvature_t = units::unit_t<
-    units::compound_unit<units::radian, units::inverse<units::meter>>>;
-
-/**
- * Represents a time-parameterized trajectory. The trajectory contains of
- * various States that represent the pose, curvature, time elapsed, velocity,
- * and acceleration at that point.
- */
-class Trajectory {
- public:
-  /**
-   * Represents one point on the trajectory.
-   */
-  struct State {
-    // The time elapsed since the beginning of the trajectory.
-    units::second_t t = 0_s;
-
-    // The speed at that point of the trajectory.
-    units::meters_per_second_t velocity = 0_mps;
-
-    // The acceleration at that point of the trajectory.
-    units::meters_per_second_squared_t acceleration = 0_mps_sq;
-
-    // The pose at that point of the trajectory.
-    Pose2d pose;
-
-    // The curvature at that point of the trajectory.
-    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.
-     * @param i The interpolant (fraction).
-     *
-     * @return The interpolated state.
-     */
-    State Interpolate(State endValue, double i) const;
-  };
-
-  Trajectory() = default;
-
-  /**
-   * Constructs a trajectory from a vector of states.
-   */
-  explicit Trajectory(const std::vector<State>& states);
-
-  /**
-   * Returns the overall duration of the trajectory.
-   * @return The duration of the trajectory.
-   */
-  units::second_t TotalTime() const { return m_totalTime; }
-
-  /**
-   * Return the states of the trajectory.
-   * @return The states of the trajectory.
-   */
-  const std::vector<State>& States() const { return m_states; }
-
-  /**
-   * Sample the trajectory at a point in time.
-   *
-   * @param t The point in time since the beginning of the trajectory to sample.
-   * @return The state at that point in time.
-   */
-  State Sample(units::second_t t) const;
-
-  /**
-   * Transforms all poses in the trajectory by the given transform. This is
-   * useful for converting a robot-relative trajectory into a field-relative
-   * trajectory. This works with respect to the first pose in the trajectory.
-   *
-   * @param transform The transform to transform the trajectory by.
-   * @return The transformed trajectory.
-   */
-  Trajectory TransformBy(const Transform2d& transform);
-
-  /**
-   * Transforms all poses in the trajectory so that they are relative to the
-   * given pose. This is useful for converting a field-relative trajectory
-   * into a robot-relative trajectory.
-   *
-   * @param pose The pose that is the origin of the coordinate frame that
-   *             the current trajectory will be transformed into.
-   * @return The transformed trajectory.
-   */
-  Trajectory RelativeTo(const Pose2d& pose);
-
-  /**
-   * Returns the initial pose of the trajectory.
-   *
-   * @return The initial pose of the trajectory.
-   */
-  Pose2d InitialPose() const { return Sample(0_s).pose; }
-
- private:
-  std::vector<State> m_states;
-  units::second_t m_totalTime;
-
-  /**
-   * Linearly interpolates between two values.
-   *
-   * @param startValue The start value.
-   * @param endValue The end value.
-   * @param t The fraction for interpolation.
-   *
-   * @return The interpolated value.
-   */
-  template <typename T>
-  static T Lerp(const T& startValue, const T& endValue, const double t) {
-    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
deleted file mode 100644
index 36118a0..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h
+++ /dev/null
@@ -1,165 +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 <utility>
-#include <vector>
-
-#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 {
-/**
- * Represents the configuration for generating a trajectory. This class stores
- * the start velocity, end velocity, max velocity, max acceleration, custom
- * constraints, and the reversed flag.
- *
- * The class must be constructed with a max velocity and max acceleration.
- * The other parameters (start velocity, end velocity, constraints, reversed)
- * have been defaulted to reasonable values (0, 0, {}, false). These values can
- * be changed via the SetXXX methods.
- */
-class TrajectoryConfig {
- public:
-  /**
-   * Constructs a config object.
-   * @param maxVelocity The max velocity of the trajectory.
-   * @param maxAcceleration The max acceleration of the trajectory.
-   */
-  TrajectoryConfig(units::meters_per_second_t maxVelocity,
-                   units::meters_per_second_squared_t maxAcceleration)
-      : m_maxVelocity(maxVelocity), m_maxAcceleration(maxAcceleration) {}
-
-  TrajectoryConfig(const TrajectoryConfig&) = delete;
-  TrajectoryConfig& operator=(const TrajectoryConfig&) = delete;
-
-  TrajectoryConfig(TrajectoryConfig&&) = default;
-  TrajectoryConfig& operator=(TrajectoryConfig&&) = default;
-
-  /**
-   * Sets the start velocity of the trajectory.
-   * @param startVelocity The start velocity of the trajectory.
-   */
-  void SetStartVelocity(units::meters_per_second_t startVelocity) {
-    m_startVelocity = startVelocity;
-  }
-
-  /**
-   * Sets the end velocity of the trajectory.
-   * @param endVelocity The end velocity of the trajectory.
-   */
-  void SetEndVelocity(units::meters_per_second_t endVelocity) {
-    m_endVelocity = endVelocity;
-  }
-
-  /**
-   * Sets the reversed flag of the trajectory.
-   * @param reversed Whether the trajectory should be reversed or not.
-   */
-  void SetReversed(bool reversed) { m_reversed = reversed; }
-
-  /**
-   * Adds a user-defined constraint to the trajectory.
-   * @param constraint The user-defined constraint.
-   */
-  template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
-                                     TrajectoryConstraint, Constraint>>>
-  void AddConstraint(Constraint constraint) {
-    m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
-  }
-
-  /**
-   * Adds a differential drive kinematics constraint to ensure that
-   * no wheel velocity of a differential drive goes above the max velocity.
-   *
-   * @param kinematics The differential drive kinematics.
-   */
-  void SetKinematics(const DifferentialDriveKinematics& kinematics) {
-    AddConstraint(
-        DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
-  }
-
-  /**
-   * 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.
-   */
-  units::meters_per_second_t StartVelocity() const { return m_startVelocity; }
-
-  /**
-   * Returns the ending velocity of the trajectory.
-   * @return The ending velocity of the trajectory.
-   */
-  units::meters_per_second_t EndVelocity() const { return m_endVelocity; }
-
-  /**
-   * Returns the maximum velocity of the trajectory.
-   * @return The maximum velocity of the trajectory.
-   */
-  units::meters_per_second_t MaxVelocity() const { return m_maxVelocity; }
-
-  /**
-   * Returns the maximum acceleration of the trajectory.
-   * @return The maximum acceleration of the trajectory.
-   */
-  units::meters_per_second_squared_t MaxAcceleration() const {
-    return m_maxAcceleration;
-  }
-
-  /**
-   * Returns the user-defined constraints of the trajectory.
-   * @return The user-defined constraints of the trajectory.
-   */
-  const std::vector<std::unique_ptr<TrajectoryConstraint>>& Constraints()
-      const {
-    return m_constraints;
-  }
-
-  /**
-   * Returns whether the trajectory is reversed or not.
-   * @return whether the trajectory is reversed or not.
-   */
-  bool IsReversed() const { return m_reversed; }
-
- private:
-  units::meters_per_second_t m_startVelocity = 0_mps;
-  units::meters_per_second_t m_endVelocity = 0_mps;
-  units::meters_per_second_t m_maxVelocity;
-  units::meters_per_second_squared_t m_maxAcceleration;
-  std::vector<std::unique_ptr<TrajectoryConstraint>> m_constraints;
-  bool m_reversed = false;
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
deleted file mode 100644
index 40e7e7b..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
+++ /dev/null
@@ -1,120 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 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 <utility>
-#include <vector>
-
-#include "frc/spline/SplineParameterizer.h"
-#include "frc/trajectory/Trajectory.h"
-#include "frc/trajectory/TrajectoryConfig.h"
-#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-namespace frc {
-/**
- * Helper class used to generate trajectories with various constraints.
- */
-class TrajectoryGenerator {
- public:
-  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
-
-  /**
-   * Generates a trajectory from the given control vectors and config. This
-   * method uses clamped cubic splines -- a method in which the exterior control
-   * vectors and interior waypoints are provided. The headings are automatically
-   * determined at the interior points to ensure continuous curvature.
-   *
-   * @param initial           The initial control vector.
-   * @param interiorWaypoints The interior waypoints.
-   * @param end               The ending control vector.
-   * @param config            The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  static Trajectory GenerateTrajectory(
-      Spline<3>::ControlVector initial,
-      const std::vector<Translation2d>& interiorWaypoints,
-      Spline<3>::ControlVector end, const TrajectoryConfig& config);
-
-  /**
-   * Generates a trajectory from the given waypoints and config. This method
-   * uses clamped cubic splines -- a method in which the initial pose, final
-   * pose, and interior waypoints are provided.  The headings are automatically
-   * determined at the interior points to ensure continuous curvature.
-   *
-   * @param start             The starting pose.
-   * @param interiorWaypoints The interior waypoints.
-   * @param end               The ending pose.
-   * @param config            The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  static Trajectory GenerateTrajectory(
-      const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
-      const Pose2d& end, const TrajectoryConfig& config);
-
-  /**
-   * Generates a trajectory from the given quintic control vectors and config.
-   * This method uses quintic hermite splines -- therefore, all points must be
-   * represented by control vectors. Continuous curvature is guaranteed in this
-   * method.
-   *
-   * @param controlVectors List of quintic control vectors.
-   * @param config         The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  static Trajectory GenerateTrajectory(
-      std::vector<Spline<5>::ControlVector> controlVectors,
-      const TrajectoryConfig& config);
-
-  /**
-   * Generates a trajectory from the given waypoints and config. This method
-   * uses quintic hermite splines -- therefore, all points must be represented
-   * by Pose2d objects. Continuous curvature is guaranteed in this method.
-   *
-   * @param waypoints List of waypoints..
-   * @param config    The configuration for the trajectory.
-   * @return The generated trajectory.
-   */
-  static Trajectory GenerateTrajectory(const std::vector<Pose2d>& waypoints,
-                                       const TrajectoryConfig& config);
-
-  /**
-   * Generate spline points from a vector of splines by parameterizing the
-   * splines.
-   *
-   * @param splines The splines to parameterize.
-   *
-   * @return The spline points for use in time parameterization of a trajectory.
-   */
-  template <typename Spline>
-  static std::vector<PoseWithCurvature> SplinePointsFromSplines(
-      const std::vector<Spline>& splines) {
-    // Create the vector of spline points.
-    std::vector<PoseWithCurvature> splinePoints;
-
-    // Add the first point to the vector.
-    splinePoints.push_back(splines.front().GetPoint(0.0));
-
-    // Iterate through the vector and parameterize each spline, adding the
-    // parameterized points to the final vector.
-    for (auto&& spline : splines) {
-      auto points = SplineParameterizer::Parameterize(spline);
-      // Append the array of poses to the vector. We are removing the first
-      // point because it's a duplicate of the last point from the previous
-      // spline.
-      splinePoints.insert(std::end(splinePoints), std::begin(points) + 1,
-                          std::end(points));
-    }
-    return splinePoints;
-  }
-
- private:
-  static const Trajectory kDoNothingTrajectory;
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
deleted file mode 100644
index b8ea8da..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
+++ /dev/null
@@ -1,107 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-/*
- * MIT License
- *
- * Copyright (c) 2018 Team 254
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
- * SOFTWARE.
- */
-
-#pragma once
-
-#include <memory>
-#include <utility>
-#include <vector>
-
-#include "frc/trajectory/Trajectory.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-namespace frc {
-/**
- * Class used to parameterize a trajectory by time.
- */
-class TrajectoryParameterizer {
- public:
-  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
-
-  /**
-   * Parameterize the trajectory by time. This is where the velocity profile is
-   * generated.
-   *
-   * The derivation of the algorithm used can be found here:
-   * <http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf>
-   *
-   * @param points Reference to the spline points.
-   * @param constraints A vector of various velocity and acceleration
-   * constraints.
-   * @param startVelocity The start velocity for the trajectory.
-   * @param endVelocity The end velocity for the trajectory.
-   * @param maxVelocity The max velocity for the trajectory.
-   * @param maxAcceleration The max acceleration for the trajectory.
-   * @param reversed Whether the robot should move backwards. Note that the
-   * robot will still move from a -> b -> ... -> z as defined in the waypoints.
-   *
-   * @return The trajectory.
-   */
-  static Trajectory TimeParameterizeTrajectory(
-      const std::vector<PoseWithCurvature>& points,
-      const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
-      units::meters_per_second_t startVelocity,
-      units::meters_per_second_t endVelocity,
-      units::meters_per_second_t maxVelocity,
-      units::meters_per_second_squared_t maxAcceleration, bool reversed);
-
- private:
-  constexpr static double kEpsilon = 1E-6;
-
-  /**
-   * Represents a constrained state that is used when time parameterizing a
-   * trajectory. Each state has the pose, curvature, distance from the start of
-   * the trajectory, max velocity, min acceleration and max acceleration.
-   */
-  struct ConstrainedState {
-    PoseWithCurvature pose = {Pose2d(), curvature_t(0.0)};
-    units::meter_t distance = 0_m;
-    units::meters_per_second_t maxVelocity = 0_mps;
-    units::meters_per_second_squared_t minAcceleration = 0_mps_sq;
-    units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
-  };
-
-  /**
-   * Enforces acceleration limits as defined by the constraints. This function
-   * is used when time parameterizing a trajectory.
-   *
-   * @param reverse Whether the robot is traveling backwards.
-   * @param constraints A vector of the user-defined velocity and acceleration
-   * constraints.
-   * @param state Pointer to the constrained state that we are operating on.
-   * This is mutated in place.
-   */
-  static void EnforceAccelerationLimits(
-      bool reverse,
-      const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
-      ConstrainedState* state);
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h
deleted file mode 100644
index 700ed9c..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryUtil.h
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#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
deleted file mode 100644
index 575088e..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ /dev/null
@@ -1,157 +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 <hal/FRCUsageReporting.h>
-#include <units/units.h>
-
-namespace frc {
-
-/**
- * A trapezoid-shaped velocity profile.
- *
- * While this class can be used for a profiled movement from start to finish,
- * the intended usage is to filter a reference's dynamics based on trapezoidal
- * velocity constraints. To compute the reference obeying this constraint, do
- * the following.
- *
- * Initialization:
- * @code{.cpp}
- * TrapezoidalMotionProfile::Constraints constraints{kMaxV, kMaxA};
- * double previousProfiledReference = initialReference;
- * @endcode
- *
- * Run on update:
- * @code{.cpp}
- * TrapezoidalMotionProfile profile{constraints, unprofiledReference,
- *                                  previousProfiledReference};
- * previousProfiledReference = profile.Calculate(timeSincePreviousUpdate);
- * @endcode
- *
- * where `unprofiledReference` is free to change between calls. Note that when
- * the unprofiled reference is within the constraints, `Calculate()` returns the
- * unprofiled reference unchanged.
- *
- * Otherwise, a timer can be started to provide monotonic values for
- * `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:
-    Constraints() {
-      HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
-    }
-    Constraints(Velocity_t maxVelocity_, Acceleration_t maxAcceleration_)
-        : maxVelocity{maxVelocity_}, maxAcceleration{maxAcceleration_} {
-      HAL_Report(HALUsageReporting::kResourceType_TrapezoidProfile, 1);
-    }
-    Velocity_t maxVelocity{0};
-    Acceleration_t maxAcceleration{0};
-  };
-
-  class State {
-   public:
-    Distance_t position{0};
-    Velocity_t velocity{0};
-    bool operator==(const State& rhs) const {
-      return position == rhs.position && velocity == rhs.velocity;
-    }
-    bool operator!=(const State& rhs) const { return !(*this == rhs); }
-  };
-
-  /**
-   * Construct a TrapezoidProfile.
-   *
-   * @param constraints The constraints on the profile, like maximum velocity.
-   * @param goal        The desired state when the profile is complete.
-   * @param initial     The initial state (usually the current state).
-   */
-  TrapezoidProfile(Constraints constraints, State goal,
-                   State initial = State{Distance_t(0), Velocity_t(0)});
-
-  TrapezoidProfile(const TrapezoidProfile&) = default;
-  TrapezoidProfile& operator=(const TrapezoidProfile&) = default;
-  TrapezoidProfile(TrapezoidProfile&&) = default;
-  TrapezoidProfile& operator=(TrapezoidProfile&&) = default;
-
-  /**
-   * Calculate the correct position and velocity for the profile at a time t
-   * where the beginning of the profile was at time t = 0.
-   *
-   * @param t The time since the beginning of the profile.
-   */
-  State Calculate(units::second_t t) const;
-
-  /**
-   * Returns the time left until a target distance in the profile is reached.
-   *
-   * @param target The target distance.
-   */
-  units::second_t TimeLeftUntil(Distance_t target) const;
-
-  /**
-   * Returns the total time the profile takes to reach the goal.
-   */
-  units::second_t TotalTime() const { return m_endDeccel; }
-
-  /**
-   * Returns true if the profile has reached the goal.
-   *
-   * The profile has reached the goal if the time since the profile started
-   * has exceeded the profile's total time.
-   *
-   * @param t The time since the beginning of the profile.
-   */
-  bool IsFinished(units::second_t t) const { return t >= TotalTime(); }
-
- private:
-  /**
-   * Returns true if the profile inverted.
-   *
-   * The profile is inverted if goal position is less than the initial position.
-   *
-   * @param initial The initial state (usually the current state).
-   * @param goal    The desired state when the profile is complete.
-   */
-  static bool ShouldFlipAcceleration(const State& initial, const State& goal) {
-    return initial.position > goal.position;
-  }
-
-  // Flip the sign of the velocity and position if the profile is inverted
-  State Direct(const State& in) const {
-    State result = in;
-    result.position *= m_direction;
-    result.velocity *= m_direction;
-    return result;
-  }
-
-  // The direction of the profile, either 1 for forwards or -1 for inverted
-  int m_direction;
-
-  Constraints m_constraints;
-  State m_initial;
-  State m_goal;
-
-  units::second_t m_endAccel;
-  units::second_t m_endFullSpeed;
-  units::second_t m_endDeccel;
-};
-}  // namespace frc
-
-#include "TrapezoidProfile.inc"
diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
deleted file mode 100644
index 50f232d..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
+++ /dev/null
@@ -1,163 +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 <algorithm>
-
-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)),
-      m_goal(Direct(goal)) {
-  if (m_initial.velocity > m_constraints.maxVelocity) {
-    m_initial.velocity = m_constraints.maxVelocity;
-  }
-
-  // Deal with a possibly truncated motion profile (with nonzero initial or
-  // final velocity) by calculating the parameters as if the profile began and
-  // ended at zero velocity
-  units::second_t cutoffBegin =
-      m_initial.velocity / m_constraints.maxAcceleration;
-  Distance_t cutoffDistBegin =
-      cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
-
-  units::second_t cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
-  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
-
-  Distance_t fullTrapezoidDist =
-      cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
-  units::second_t accelerationTime =
-      m_constraints.maxVelocity / m_constraints.maxAcceleration;
-
-  Distance_t fullSpeedDist =
-      fullTrapezoidDist -
-      accelerationTime * accelerationTime * m_constraints.maxAcceleration;
-
-  // Handle the case where the profile never reaches full speed
-  if (fullSpeedDist < Distance_t(0)) {
-    accelerationTime =
-        units::math::sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
-    fullSpeedDist = Distance_t(0);
-  }
-
-  m_endAccel = accelerationTime - cutoffBegin;
-  m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
-  m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
-}
-
-template <class Distance>
-typename TrapezoidProfile<Distance>::State
-TrapezoidProfile<Distance>::Calculate(units::second_t t) const {
-  State result = m_initial;
-
-  if (t < m_endAccel) {
-    result.velocity += t * m_constraints.maxAcceleration;
-    result.position +=
-        (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
-  } else if (t < m_endFullSpeed) {
-    result.velocity = m_constraints.maxVelocity;
-    result.position += (m_initial.velocity +
-                        m_endAccel * m_constraints.maxAcceleration / 2.0) *
-                           m_endAccel +
-                       m_constraints.maxVelocity * (t - m_endAccel);
-  } else if (t <= m_endDeccel) {
-    result.velocity =
-        m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
-    units::second_t timeLeft = m_endDeccel - t;
-    result.position =
-        m_goal.position -
-        (m_goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) *
-            timeLeft;
-  } else {
-    result = m_goal;
-  }
-
-  return Direct(result);
-}
-
-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;
-
-  if (target < position) {
-    endAccel *= -1.0;
-    endFullSpeed *= -1.0;
-    velocity *= -1.0;
-  }
-
-  endAccel = units::math::max(endAccel, 0_s);
-  endFullSpeed = units::math::max(endFullSpeed, 0_s);
-  units::second_t endDeccel = m_endDeccel - endAccel - endFullSpeed;
-  endDeccel = units::math::max(endDeccel, 0_s);
-
-  const Acceleration_t acceleration = m_constraints.maxAcceleration;
-  const Acceleration_t decceleration = -m_constraints.maxAcceleration;
-
-  Distance_t distToTarget = units::math::abs(target - position);
-
-  if (distToTarget < Distance_t(1e-6)) {
-    return 0_s;
-  }
-
-  Distance_t accelDist =
-      velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
-
-  Velocity_t deccelVelocity;
-  if (endAccel > 0_s) {
-    deccelVelocity = units::math::sqrt(
-        units::math::abs(velocity * velocity + 2 * acceleration * accelDist));
-  } else {
-    deccelVelocity = velocity;
-  }
-
-  Distance_t deccelDist =
-      deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
-
-  deccelDist = units::math::max(deccelDist, Distance_t(0));
-
-  Distance_t fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
-
-  if (accelDist > distToTarget) {
-    accelDist = distToTarget;
-    fullSpeedDist = Distance_t(0);
-    deccelDist = Distance_t(0);
-  } else if (accelDist + fullSpeedDist > distToTarget) {
-    fullSpeedDist = distToTarget - accelDist;
-    deccelDist = Distance_t(0);
-  } else {
-    deccelDist = distToTarget - fullSpeedDist - accelDist;
-  }
-
-  units::second_t accelTime =
-      (-velocity + units::math::sqrt(units::math::abs(
-                       velocity * velocity + 2 * acceleration * accelDist))) /
-      acceleration;
-
-  units::second_t deccelTime =
-      (-deccelVelocity +
-       units::math::sqrt(units::math::abs(deccelVelocity * deccelVelocity +
-                                          2 * decceleration * deccelDist))) /
-      decceleration;
-
-  units::second_t fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
-
-  return accelTime + fullSpeedTime + deccelTime;
-}
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
deleted file mode 100644
index de25738..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
+++ /dev/null
@@ -1,40 +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/trajectory/constraint/TrajectoryConstraint.h"
-
-namespace frc {
-
-/**
- * A constraint on the maximum absolute centripetal acceleration allowed when
- * traversing a trajectory. The centripetal acceleration of a robot is defined
- * as the velocity squared divided by the radius of curvature.
- *
- * Effectively, limiting the maximum centripetal acceleration will cause the
- * robot to slow down around tight turns, making it easier to track trajectories
- * with sharp turns.
- */
-class CentripetalAccelerationConstraint : public TrajectoryConstraint {
- public:
-  explicit CentripetalAccelerationConstraint(
-      units::meters_per_second_squared_t maxCentripetalAcceleration);
-
-  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:
-  units::meters_per_second_squared_t m_maxCentripetalAcceleration;
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
deleted file mode 100644
index 6259c96..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
+++ /dev/null
@@ -1,38 +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/kinematics/DifferentialDriveKinematics.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 DifferentialDriveKinematicsConstraint : public TrajectoryConstraint {
- public:
-  DifferentialDriveKinematicsConstraint(DifferentialDriveKinematics 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:
-  DifferentialDriveKinematics m_kinematics;
-  units::meters_per_second_t m_maxSpeed;
-};
-}  // 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
deleted file mode 100644
index 50ace85..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.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 "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
deleted file mode 100644
index 53e3953..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
+++ /dev/null
@@ -1,40 +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 <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
deleted file mode 100644
index a8ad870..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
+++ /dev/null
@@ -1,45 +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 <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
deleted file mode 100644
index c3437d5..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
+++ /dev/null
@@ -1,49 +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
-
-/**
- * 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/frc/trajectory/constraint/TrajectoryConstraint.h b/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
deleted file mode 100644
index dcde8c4..0000000
--- a/wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.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 <limits>
-
-#include <units/units.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/spline/Spline.h"
-
-namespace frc {
-/**
- * An interface for defining user-defined velocity and acceleration constraints
- * while generating trajectories.
- */
-class TrajectoryConstraint {
- public:
-  TrajectoryConstraint() = default;
-
-  TrajectoryConstraint(const TrajectoryConstraint&) = default;
-  TrajectoryConstraint& operator=(const TrajectoryConstraint&) = default;
-
-  TrajectoryConstraint(TrajectoryConstraint&&) = default;
-  TrajectoryConstraint& operator=(TrajectoryConstraint&&) = default;
-
-  virtual ~TrajectoryConstraint() = default;
-
-  /**
-   * Represents a minimum and maximum acceleration.
-   */
-  struct MinMax {
-    /**
-     * The minimum acceleration.
-     */
-    units::meters_per_second_squared_t minAcceleration{
-        -std::numeric_limits<double>::max()};
-
-    /**
-     * The maximum acceleration.
-     */
-    units::meters_per_second_squared_t maxAcceleration{
-        std::numeric_limits<double>::max()};
-  };
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param pose The pose at the current point in the trajectory.
-   * @param curvature The curvature at the current point in the trajectory.
-   * @param velocity The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   *
-   * @return The absolute maximum velocity.
-   */
-  virtual units::meters_per_second_t MaxVelocity(
-      const Pose2d& pose, curvature_t curvature,
-      units::meters_per_second_t velocity) = 0;
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param pose The pose at the current point in the trajectory.
-   * @param curvature The curvature at the current point in the trajectory.
-   * @param speed The speed at the current point in the trajectory.
-   *
-   * @return The min and max acceleration bounds.
-   */
-  virtual MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
-                                    units::meters_per_second_t speed) = 0;
-};
-}  // namespace frc
diff --git a/wpilibc/src/main/native/include/frc/util/Color.h b/wpilibc/src/main/native/include/frc/util/Color.h
index 2538d60..1321380 100644
--- a/wpilibc/src/main/native/include/frc/util/Color.h
+++ b/wpilibc/src/main/native/include/frc/util/Color.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -399,7 +399,7 @@
   /**
    * #20B2AA.
    */
-  static const Color kLightSeagGeen;
+  static const Color kLightSeaGreen;
 
   /**
    * #87CEFA.
@@ -414,7 +414,7 @@
   /**
    * #B0C4DE.
    */
-  static const Color kLightSteellue;
+  static const Color kLightSteelBlue;
 
   /**
    * #FFFFE0.
@@ -755,6 +755,42 @@
         green(roundAndClamp(g)),
         blue(roundAndClamp(b)) {}
 
+  /**
+   * Creates a Color from HSV values.
+   *
+   * @param h The h value [0-180]
+   * @param s The s value [0-255]
+   * @param v The v value [0-255]
+   * @return The color
+   */
+  static constexpr Color FromHSV(int h, int s, int v) {
+    if (s == 0) {
+      return {v / 255.0, v / 255.0, v / 255.0};
+    }
+
+    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:
+        return Color(v / 255.0, t / 255.0, p / 255.0);
+      case 1:
+        return Color(q / 255.0, v / 255.0, p / 255.0);
+      case 2:
+        return Color(p / 255.0, v / 255.0, t / 255.0);
+      case 3:
+        return Color(p / 255.0, q / 255.0, v / 255.0);
+      case 4:
+        return Color(t / 255.0, p / 255.0, v / 255.0);
+      default:
+        return Color(v / 255.0, p / 255.0, q / 255.0);
+    }
+  }
+
   double red = 0.0;
   double green = 0.0;
   double blue = 0.0;
@@ -864,13 +900,13 @@
 inline constexpr Color Color::kLightGreen{0.5647059f, 0.93333334f, 0.5647059f};
 inline constexpr Color Color::kLightPink{1.0f, 0.7137255f, 0.75686276f};
 inline constexpr Color Color::kLightSalmon{1.0f, 0.627451f, 0.47843137f};
-inline constexpr Color Color::kLightSeagGeen{0.1254902f, 0.69803923f,
+inline constexpr Color Color::kLightSeaGreen{0.1254902f, 0.69803923f,
                                              0.6666667f};
 inline constexpr Color Color::kLightSkyBlue{0.5294118f, 0.80784315f,
                                             0.98039216f};
 inline constexpr Color Color::kLightSlateGray{0.46666667f, 0.53333336f, 0.6f};
-inline constexpr Color Color::kLightSteellue{0.6901961f, 0.76862746f,
-                                             0.87058824f};
+inline constexpr Color Color::kLightSteelBlue{0.6901961f, 0.76862746f,
+                                              0.87058824f};
 inline constexpr Color Color::kLightYellow{1.0f, 1.0f, 0.8784314f};
 inline constexpr Color Color::kLime{0.0f, 1.0f, 0.0f};
 inline constexpr Color Color::kLimeGreen{0.19607843f, 0.8039216f, 0.19607843f};
diff --git a/wpilibc/src/main/native/include/frc2/Timer.h b/wpilibc/src/main/native/include/frc2/Timer.h
index c1eeb16..8f943a6 100644
--- a/wpilibc/src/main/native/include/frc2/Timer.h
+++ b/wpilibc/src/main/native/include/frc2/Timer.h
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2008-2020 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,9 @@
 
 #pragma once
 
-#include <units/units.h>
-#include <wpi/deprecated.h>
+#include <units/time.h>
 #include <wpi/mutex.h>
 
-#include "frc/Base.h"
-
 namespace frc2 {
 
 /**
@@ -75,7 +72,8 @@
    * Start the timer running.
    *
    * Just set the running flag to true indicating that all time requests should
-   * be relative to the system clock.
+   * be relative to the system clock. Note that this method is a no-op if the
+   * timer is already running.
    */
   void Start();
 
@@ -89,6 +87,14 @@
   void Stop();
 
   /**
+   * Check if the period specified has passed.
+   *
+   * @param seconds The period to check.
+   * @return        True if the period has passed.
+   */
+  bool HasElapsed(units::second_t period) const;
+
+  /**
    * Check if the period specified has passed and if it has, advance the start
    * time by that period. This is useful to decide if it's time to do periodic
    * work without drifting later by the time it took to get around to checking.
@@ -99,6 +105,16 @@
   bool HasPeriodPassed(units::second_t period);
 
   /**
+   * Check if the period specified has passed and if it has, advance the start
+   * time by that period. This is useful to decide if it's time to do periodic
+   * work without drifting later by the time it took to get around to checking.
+   *
+   * @param period The period to check for.
+   * @return       True if the period has passed.
+   */
+  bool AdvanceIfElapsed(units::second_t period);
+
+  /**
    * Return the FPGA system clock time in seconds.
    *
    * Return the time from the FPGA hardware clock in seconds since the FPGA
@@ -125,9 +141,6 @@
    */
   static units::second_t GetMatchTime();
 
-  // The time, in seconds, at which the 32-bit FPGA timestamp rolls over to 0
-  static const units::second_t kRolloverTime;
-
  private:
   units::second_t m_startTime = 0_s;
   units::second_t m_accumulatedTime = 0_s;
diff --git a/wpilibc/src/test/native/cpp/DriverStationTest.cpp b/wpilibc/src/test/native/cpp/DriverStationTest.cpp
new file mode 100644
index 0000000..3b8272f
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/DriverStationTest.cpp
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <string>
+#include <tuple>
+
+#include "frc/DriverStation.h"
+#include "frc/Joystick.h"
+#include "frc/simulation/DriverStationSim.h"
+#include "frc/simulation/SimHooks.h"
+#include "gtest/gtest.h"
+
+class IsJoystickConnectedParametersTests
+    : public ::testing::TestWithParam<std::tuple<int, int, int, bool>> {};
+
+TEST_P(IsJoystickConnectedParametersTests, IsJoystickConnected) {
+  frc::sim::DriverStationSim::SetJoystickAxisCount(1, std::get<0>(GetParam()));
+  frc::sim::DriverStationSim::SetJoystickButtonCount(1,
+                                                     std::get<1>(GetParam()));
+  frc::sim::DriverStationSim::SetJoystickPOVCount(1, std::get<2>(GetParam()));
+  frc::sim::DriverStationSim::NotifyNewData();
+
+  ASSERT_EQ(std::get<3>(GetParam()),
+            frc::DriverStation::GetInstance().IsJoystickConnected(1));
+}
+
+INSTANTIATE_TEST_SUITE_P(IsConnectedTests, IsJoystickConnectedParametersTests,
+                         ::testing::Values(std::make_tuple(0, 0, 0, false),
+                                           std::make_tuple(1, 0, 0, true),
+                                           std::make_tuple(0, 1, 0, true),
+                                           std::make_tuple(0, 0, 1, true),
+                                           std::make_tuple(1, 1, 1, true),
+                                           std::make_tuple(4, 10, 1, true)));
+class JoystickConnectionWarningTests
+    : public ::testing::TestWithParam<
+          std::tuple<bool, bool, bool, std::string>> {};
+
+TEST_P(JoystickConnectionWarningTests, JoystickConnectionWarnings) {
+  // Capture all output to stderr.
+  ::testing::internal::CaptureStderr();
+
+  // Set FMS and Silence settings
+  frc::sim::DriverStationSim::SetFmsAttached(std::get<0>(GetParam()));
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::DriverStation::GetInstance().SilenceJoystickConnectionWarning(
+      std::get<1>(GetParam()));
+
+  // Create joystick and attempt to retrieve button.
+  frc::Joystick joystick(0);
+  joystick.GetRawButton(1);
+
+  frc::sim::StepTiming(1_s);
+  EXPECT_EQ(
+      frc::DriverStation::GetInstance().IsJoystickConnectionWarningSilenced(),
+      std::get<2>(GetParam()));
+  EXPECT_EQ(::testing::internal::GetCapturedStderr(), std::get<3>(GetParam()));
+}
+
+INSTANTIATE_TEST_SUITE_P(
+    DriverStation, JoystickConnectionWarningTests,
+    ::testing::Values(std::make_tuple(false, true, true, ""),
+                      std::make_tuple(false, false, false,
+                                      "Joystick Button missing, check if all "
+                                      "controllers are plugged in\n"),
+                      std::make_tuple(true, true, false,
+                                      "Joystick Button missing, check if all "
+                                      "controllers are plugged in\n"),
+                      std::make_tuple(true, false, false,
+                                      "Joystick Button missing, check if all "
+                                      "controllers are plugged in\n")));
diff --git a/wpilibc/src/test/native/cpp/JoystickTest.cpp b/wpilibc/src/test/native/cpp/JoystickTest.cpp
new file mode 100644
index 0000000..0480b76
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/JoystickTest.cpp
@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/Joystick.h"  // NOLINT(build/include_order)
+
+#include "frc/simulation/JoystickSim.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(JoystickTests, GetX) {
+  Joystick joy{1};
+  sim::JoystickSim joysim{joy};
+
+  joysim.SetX(0.25);
+  joysim.NotifyNewData();
+  ASSERT_NEAR(joy.GetX(), 0.25, 0.001);
+
+  joysim.SetX(0);
+  joysim.NotifyNewData();
+  ASSERT_NEAR(joy.GetX(), 0, 0.001);
+}
+
+TEST(JoystickTests, GetY) {
+  Joystick joy{1};
+  sim::JoystickSim joysim{joy};
+
+  joysim.SetY(0.25);
+  joysim.NotifyNewData();
+  ASSERT_NEAR(joy.GetY(), 0.25, 0.001);
+
+  joysim.SetY(0);
+  joysim.NotifyNewData();
+  ASSERT_NEAR(joy.GetY(), 0, 0.001);
+}
diff --git a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
deleted file mode 100644
index 0bb1002..0000000
--- a/wpilibc/src/test/native/cpp/LinearFilterNoiseTest.cpp
+++ /dev/null
@@ -1,94 +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"  // NOLINT(build/include_order)
-
-#include <cmath>
-#include <memory>
-#include <random>
-
-#include <units/units.h>
-#include <wpi/math>
-
-#include "gtest/gtest.h"
-
-// Filter constants
-static constexpr units::second_t kFilterStep = 0.005_s;
-static constexpr units::second_t kFilterTime = 2.0_s;
-static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
-static constexpr int32_t kMovAvgTaps = 6;
-
-enum LinearFilterNoiseTestType { TEST_SINGLE_POLE_IIR, TEST_MOVAVG };
-
-std::ostream& operator<<(std::ostream& os,
-                         const LinearFilterNoiseTestType& type) {
-  switch (type) {
-    case TEST_SINGLE_POLE_IIR:
-      os << "LinearFilter SinglePoleIIR";
-      break;
-    case TEST_MOVAVG:
-      os << "LinearFilter MovingAverage";
-      break;
-  }
-
-  return os;
-}
-
-static double GetData(double t) {
-  return 100.0 * std::sin(2.0 * wpi::math::pi * t);
-}
-
-class LinearFilterNoiseTest
-    : public testing::TestWithParam<LinearFilterNoiseTestType> {
- protected:
-  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<double>>(
-            frc::LinearFilter<double>::SinglePoleIIR(kSinglePoleIIRTimeConstant,
-                                                     kFilterStep));
-        break;
-      }
-
-      case TEST_MOVAVG: {
-        m_filter = std::make_unique<frc::LinearFilter<double>>(
-            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
-        break;
-      }
-    }
-  }
-};
-
-/**
- * Test if the filter reduces the noise produced by a signal generator
- */
-TEST_P(LinearFilterNoiseTest, NoiseReduce) {
-  double noiseGenError = 0.0;
-  double filterError = 0.0;
-
-  std::random_device rd;
-  std::mt19937 gen{rd()};
-  std::normal_distribution<double> distr{0.0, 10.0};
-
-  for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
-    double theory = GetData(t.to<double>());
-    double noise = distr(gen);
-    filterError += std::abs(m_filter->Calculate(theory + noise) - theory);
-    noiseGenError += std::abs(noise - theory);
-  }
-
-  RecordProperty("FilterError", filterError);
-
-  // The filter should have produced values closer to the theory
-  EXPECT_GT(noiseGenError, filterError)
-      << "Filter should have reduced noise accumulation but failed";
-}
-
-INSTANTIATE_TEST_SUITE_P(Test, LinearFilterNoiseTest,
-                         testing::Values(TEST_SINGLE_POLE_IIR, TEST_MOVAVG));
diff --git a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
deleted file mode 100644
index 8db91b3..0000000
--- a/wpilibc/src/test/native/cpp/LinearFilterOutputTest.cpp
+++ /dev/null
@@ -1,136 +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"  // NOLINT(build/include_order)
-
-#include <cmath>
-#include <functional>
-#include <memory>
-#include <random>
-
-#include <units/units.h>
-#include <wpi/math>
-
-#include "gtest/gtest.h"
-
-// Filter constants
-static constexpr units::second_t kFilterStep = 0.005_s;
-static constexpr units::second_t kFilterTime = 2.0_s;
-static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
-static constexpr double kSinglePoleIIRExpectedOutput = -3.2172003;
-static constexpr double kHighPassTimeConstant = 0.006631;
-static constexpr double kHighPassExpectedOutput = 10.074717;
-static constexpr int32_t kMovAvgTaps = 6;
-static constexpr double kMovAvgExpectedOutput = -10.191644;
-
-enum LinearFilterOutputTestType {
-  TEST_SINGLE_POLE_IIR,
-  TEST_HIGH_PASS,
-  TEST_MOVAVG,
-  TEST_PULSE
-};
-
-std::ostream& operator<<(std::ostream& os,
-                         const LinearFilterOutputTestType& type) {
-  switch (type) {
-    case TEST_SINGLE_POLE_IIR:
-      os << "LinearFilter SinglePoleIIR";
-      break;
-    case TEST_HIGH_PASS:
-      os << "LinearFilter HighPass";
-      break;
-    case TEST_MOVAVG:
-      os << "LinearFilter MovingAverage";
-      break;
-    case TEST_PULSE:
-      os << "LinearFilter Pulse";
-      break;
-  }
-
-  return os;
-}
-
-static double GetData(double t) {
-  return 100.0 * std::sin(2.0 * wpi::math::pi * t) +
-         20.0 * std::cos(50.0 * wpi::math::pi * t);
-}
-
-static double GetPulseData(double t) {
-  if (std::abs(t - 1.0) < 0.001) {
-    return 1.0;
-  } else {
-    return 0.0;
-  }
-}
-
-/**
- * A fixture that includes a consistent data source wrapped in a filter
- */
-class LinearFilterOutputTest
-    : public testing::TestWithParam<LinearFilterOutputTestType> {
- protected:
-  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<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<double>>(
-            frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
-                                                kFilterStep));
-        m_data = GetData;
-        m_expectedOutput = kHighPassExpectedOutput;
-        break;
-      }
-
-      case TEST_MOVAVG: {
-        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<double>>(
-            frc::LinearFilter<double>::MovingAverage(kMovAvgTaps));
-        m_data = GetPulseData;
-        m_expectedOutput = 0.0;
-        break;
-      }
-    }
-  }
-};
-
-/**
- * Test if the linear filters produce consistent output for a given data set.
- */
-TEST_P(LinearFilterOutputTest, Output) {
-  double filterOutput = 0.0;
-  for (auto t = 0_s; t < kFilterTime; t += kFilterStep) {
-    filterOutput = m_filter->Calculate(m_data(t.to<double>()));
-  }
-
-  RecordProperty("LinearFilterOutput", filterOutput);
-
-  EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
-      << "Filter output didn't match expected value";
-}
-
-INSTANTIATE_TEST_SUITE_P(Test, LinearFilterOutputTest,
-                         testing::Values(TEST_SINGLE_POLE_IIR, TEST_HIGH_PASS,
-                                         TEST_MOVAVG, TEST_PULSE));
diff --git a/wpilibc/src/test/native/cpp/MedianFilterTest.cpp b/wpilibc/src/test/native/cpp/MedianFilterTest.cpp
deleted file mode 100644
index 7fe7d2e..0000000
--- a/wpilibc/src/test/native/cpp/MedianFilterTest.cpp
+++ /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/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/ScopedTracerTest.cpp b/wpilibc/src/test/native/cpp/ScopedTracerTest.cpp
new file mode 100644
index 0000000..da222de
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/ScopedTracerTest.cpp
@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <string>
+#include <thread>
+
+#include <wpi/SmallString.h>
+#include <wpi/StringRef.h>
+#include <wpi/raw_ostream.h>
+
+#include "frc/ScopedTracer.h"
+#include "gtest/gtest.h"
+
+wpi::SmallString<128> buf;
+wpi::raw_svector_ostream os(buf);
+
+#ifdef __APPLE__
+TEST(ScopedTracerTest, DISABLED_Timing) {
+#else
+TEST(ScopedTracerTest, Timing) {
+#endif
+  {
+    frc::ScopedTracer tracer("timing_test", os);
+    std::this_thread::sleep_for(std::chrono::milliseconds(1500));
+  }
+
+  wpi::StringRef out = os.str();
+  EXPECT_TRUE(out.startswith("	timing_test: 1.5"));
+}
diff --git a/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp b/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
index ae253a7..dea56bb 100644
--- a/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
+++ b/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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,13 +7,18 @@
 
 #include <thread>
 
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+
 #include "frc/SlewRateLimiter.h"
+#include "frc/simulation/SimHooks.h"
 #include "gtest/gtest.h"
 
 TEST(SlewRateLimiterTest, SlewRateLimitTest) {
   frc::SlewRateLimiter<units::meters> limiter(1_mps);
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  frc::sim::StepTiming(1.0_s);
 
   EXPECT_TRUE(limiter.Calculate(2_m) < 2_m);
 }
@@ -21,7 +26,7 @@
 TEST(SlewRateLimiterTest, SlewRateNoLimitTest) {
   frc::SlewRateLimiter<units::meters> limiter(1_mps);
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  frc::sim::StepTiming(1.0_s);
 
   EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
 }
diff --git a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp
new file mode 100644
index 0000000..f91efb8
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp
@@ -0,0 +1,394 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/TimedRobot.h"  // NOLINT(build/include_order)
+
+#include <stdint.h>
+
+#include <atomic>
+#include <thread>
+
+#include "frc/simulation/DriverStationSim.h"
+#include "frc/simulation/SimHooks.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+namespace {
+class TimedRobotTest : public ::testing::Test {
+ protected:
+  void SetUp() override { frc::sim::PauseTiming(); }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+class MockRobot : public TimedRobot {
+ public:
+  std::atomic<uint32_t> m_robotInitCount{0};
+  std::atomic<uint32_t> m_simulationInitCount{0};
+  std::atomic<uint32_t> m_disabledInitCount{0};
+  std::atomic<uint32_t> m_autonomousInitCount{0};
+  std::atomic<uint32_t> m_teleopInitCount{0};
+  std::atomic<uint32_t> m_testInitCount{0};
+
+  std::atomic<uint32_t> m_robotPeriodicCount{0};
+  std::atomic<uint32_t> m_simulationPeriodicCount{0};
+  std::atomic<uint32_t> m_disabledPeriodicCount{0};
+  std::atomic<uint32_t> m_autonomousPeriodicCount{0};
+  std::atomic<uint32_t> m_teleopPeriodicCount{0};
+  std::atomic<uint32_t> m_testPeriodicCount{0};
+
+  void RobotInit() override { m_robotInitCount++; }
+
+  void SimulationInit() override { m_simulationInitCount++; }
+
+  void DisabledInit() override { m_disabledInitCount++; }
+
+  void AutonomousInit() override { m_autonomousInitCount++; }
+
+  void TeleopInit() override { m_teleopInitCount++; }
+
+  void TestInit() override { m_testInitCount++; }
+
+  void RobotPeriodic() override { m_robotPeriodicCount++; }
+
+  void SimulationPeriodic() override { m_simulationPeriodicCount++; }
+
+  void DisabledPeriodic() override { m_disabledPeriodicCount++; }
+
+  void AutonomousPeriodic() override { m_autonomousPeriodicCount++; }
+
+  void TeleopPeriodic() override { m_teleopPeriodicCount++; }
+
+  void TestPeriodic() override { m_testPeriodicCount++; }
+};
+}  // namespace
+
+TEST_F(TimedRobotTest, Disabled) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(2u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, Autonomous) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(true);
+  frc::sim::DriverStationSim::SetTest(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(1u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(2u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, Teleop) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetTest(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(1u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(1u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(1u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(2u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, Test) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetTest(true);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(0u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(1u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(1u, robot.m_testPeriodicCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_robotInitCount);
+  EXPECT_EQ(1u, robot.m_simulationInitCount);
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_autonomousInitCount);
+  EXPECT_EQ(0u, robot.m_teleopInitCount);
+  EXPECT_EQ(1u, robot.m_testInitCount);
+
+  EXPECT_EQ(2u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(2u, robot.m_simulationPeriodicCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, robot.m_autonomousPeriodicCount);
+  EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
+  EXPECT_EQ(2u, robot.m_testPeriodicCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, AddPeriodic) {
+  MockRobot robot;
+
+  std::atomic<uint32_t> callbackCount{0};
+  robot.AddPeriodic([&] { callbackCount++; }, 10_ms);
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, callbackCount);
+
+  frc::sim::StepTiming(10_ms);
+
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(1u, callbackCount);
+
+  frc::sim::StepTiming(10_ms);
+
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(2u, callbackCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, AddPeriodicWithOffset) {
+  MockRobot robot;
+
+  std::atomic<uint32_t> callbackCount{0};
+  robot.AddPeriodic([&] { callbackCount++; }, 10_ms, 5_ms);
+
+  // Expirations in this test (ms)
+  //
+  // Robot | Callback
+  // ================
+  //    20 |      15
+  //    40 |      25
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, callbackCount);
+
+  frc::sim::StepTiming(7.5_ms);
+
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(0u, callbackCount);
+
+  frc::sim::StepTiming(7.5_ms);
+
+  EXPECT_EQ(0u, robot.m_disabledInitCount);
+  EXPECT_EQ(0u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(1u, callbackCount);
+
+  frc::sim::StepTiming(5_ms);
+
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(1u, callbackCount);
+
+  frc::sim::StepTiming(5_ms);
+
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_disabledPeriodicCount);
+  EXPECT_EQ(2u, callbackCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
diff --git a/wpilibc/src/test/native/cpp/WatchdogTest.cpp b/wpilibc/src/test/native/cpp/WatchdogTest.cpp
index 10ff996..0e33511 100644
--- a/wpilibc/src/test/native/cpp/WatchdogTest.cpp
+++ b/wpilibc/src/test/native/cpp/WatchdogTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2018-2020 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,101 +9,92 @@
 
 #include <stdint.h>
 
-#include <thread>
-
-#include <wpi/raw_ostream.h>
-
+#include "frc/simulation/SimHooks.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_EnableDisable) {
-#else
-TEST(WatchdogTest, EnableDisable) {
-#endif
+namespace {
+class WatchdogTest : public ::testing::Test {
+ protected:
+  void SetUp() override { frc::sim::PauseTiming(); }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+}  // namespace
+
+TEST_F(WatchdogTest, EnableDisable) {
   uint32_t watchdogCounter = 0;
 
   Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
 
-  wpi::outs() << "Run 1\n";
+  // Run 1
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Disable();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 
-  wpi::outs() << "Run 2\n";
+  // Run 2
   watchdogCounter = 0;
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(600));
+  frc::sim::StepTiming(0.4_s);
   watchdog.Disable();
 
   EXPECT_EQ(1u, watchdogCounter)
       << "Watchdog either didn't trigger or triggered more than once";
 
-  wpi::outs() << "Run 3\n";
+  // Run 3
   watchdogCounter = 0;
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+  frc::sim::StepTiming(1_s);
   watchdog.Disable();
 
   EXPECT_EQ(1u, watchdogCounter)
       << "Watchdog either didn't trigger or triggered more than once";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_Reset) {
-#else
-TEST(WatchdogTest, Reset) {
-#endif
+TEST_F(WatchdogTest, Reset) {
   uint32_t watchdogCounter = 0;
 
   Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
 
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Reset();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Disable();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_SetTimeout) {
-#else
-TEST(WatchdogTest, SetTimeout) {
-#endif
+TEST_F(WatchdogTest, SetTimeout) {
   uint32_t watchdogCounter = 0;
 
-  Watchdog watchdog(1.0_s, [&] { watchdogCounter++; });
+  Watchdog watchdog(1_s, [&] { watchdogCounter++; });
 
   watchdog.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.SetTimeout(0.2_s);
 
   EXPECT_EQ(0.2, watchdog.GetTimeout());
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 
-  std::this_thread::sleep_for(std::chrono::milliseconds(300));
+  frc::sim::StepTiming(0.3_s);
   watchdog.Disable();
 
   EXPECT_EQ(1u, watchdogCounter)
       << "Watchdog either didn't trigger or triggered more than once";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_IsExpired) {
-#else
-TEST(WatchdogTest, IsExpired) {
-#endif
+TEST_F(WatchdogTest, IsExpired) {
   Watchdog watchdog(0.2_s, [] {});
   EXPECT_FALSE(watchdog.IsExpired());
   watchdog.Enable();
 
   EXPECT_FALSE(watchdog.IsExpired());
-  std::this_thread::sleep_for(std::chrono::milliseconds(300));
+  frc::sim::StepTiming(0.3_s);
   EXPECT_TRUE(watchdog.IsExpired());
 
   watchdog.Disable();
@@ -113,43 +104,35 @@
   EXPECT_FALSE(watchdog.IsExpired());
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_Epochs) {
-#else
-TEST(WatchdogTest, Epochs) {
-#endif
+TEST_F(WatchdogTest, Epochs) {
   uint32_t watchdogCounter = 0;
 
   Watchdog watchdog(0.4_s, [&] { watchdogCounter++; });
 
-  wpi::outs() << "Run 1\n";
+  // Run 1
   watchdog.Enable();
   watchdog.AddEpoch("Epoch 1");
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  frc::sim::StepTiming(0.1_s);
   watchdog.AddEpoch("Epoch 2");
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  frc::sim::StepTiming(0.1_s);
   watchdog.AddEpoch("Epoch 3");
   watchdog.Disable();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 
-  wpi::outs() << "Run 2\n";
+  // Run 2
   watchdog.Enable();
   watchdog.AddEpoch("Epoch 1");
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.Reset();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.2_s);
   watchdog.AddEpoch("Epoch 2");
   watchdog.Disable();
 
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 }
 
-#ifdef __APPLE__
-TEST(WatchdogTest, DISABLED_MultiWatchdog) {
-#else
-TEST(WatchdogTest, MultiWatchdog) {
-#endif
+TEST_F(WatchdogTest, MultiWatchdog) {
   uint32_t watchdogCounter1 = 0;
   uint32_t watchdogCounter2 = 0;
 
@@ -157,13 +140,13 @@
   Watchdog watchdog2(0.6_s, [&] { watchdogCounter2++; });
 
   watchdog2.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(200));
+  frc::sim::StepTiming(0.25_s);
   EXPECT_EQ(0u, watchdogCounter1) << "Watchdog triggered early";
   EXPECT_EQ(0u, watchdogCounter2) << "Watchdog triggered early";
 
   // Sleep enough such that only the watchdog enabled later times out first
   watchdog1.Enable();
-  std::this_thread::sleep_for(std::chrono::milliseconds(300));
+  frc::sim::StepTiming(0.25_s);
   watchdog1.Disable();
   watchdog2.Disable();
 
diff --git a/wpilibc/src/test/native/cpp/XboxControllerTest.cpp b/wpilibc/src/test/native/cpp/XboxControllerTest.cpp
new file mode 100644
index 0000000..66ccfcd
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/XboxControllerTest.cpp
@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/XboxController.h"  // NOLINT(build/include_order)
+
+#include "frc/simulation/XboxControllerSim.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(XboxControllerTests, GetX) {
+  XboxController joy{2};
+  sim::XboxControllerSim joysim{joy};
+
+  joysim.SetX(XboxController::kLeftHand, 0.35);
+  joysim.SetX(XboxController::kRightHand, 0.45);
+  joysim.NotifyNewData();
+  ASSERT_NEAR(joy.GetX(XboxController::kLeftHand), 0.35, 0.001);
+  ASSERT_NEAR(joy.GetX(XboxController::kRightHand), 0.45, 0.001);
+}
+
+TEST(XboxControllerTests, GetBumper) {
+  XboxController joy{1};
+  sim::XboxControllerSim joysim{joy};
+
+  joysim.SetBumper(XboxController::kLeftHand, false);
+  joysim.SetBumper(XboxController::kRightHand, true);
+  joysim.NotifyNewData();
+  ASSERT_FALSE(joy.GetBumper(XboxController::kLeftHand));
+  ASSERT_TRUE(joy.GetBumper(XboxController::kRightHand));
+  // need to call pressed and released to clear flags
+  joy.GetBumperPressed(XboxController::kLeftHand);
+  joy.GetBumperReleased(XboxController::kLeftHand);
+  joy.GetBumperPressed(XboxController::kRightHand);
+  joy.GetBumperReleased(XboxController::kRightHand);
+
+  joysim.SetBumper(XboxController::kLeftHand, true);
+  joysim.SetBumper(XboxController::kRightHand, false);
+  joysim.NotifyNewData();
+  ASSERT_TRUE(joy.GetBumper(XboxController::kLeftHand));
+  ASSERT_TRUE(joy.GetBumperPressed(XboxController::kLeftHand));
+  ASSERT_FALSE(joy.GetBumperReleased(XboxController::kLeftHand));
+  ASSERT_FALSE(joy.GetBumper(XboxController::kRightHand));
+  ASSERT_FALSE(joy.GetBumperPressed(XboxController::kRightHand));
+  ASSERT_TRUE(joy.GetBumperReleased(XboxController::kRightHand));
+}
+
+TEST(XboxControllerTests, GetAButton) {
+  XboxController joy{1};
+  sim::XboxControllerSim joysim{joy};
+
+  joysim.SetAButton(false);
+  joysim.NotifyNewData();
+  ASSERT_FALSE(joy.GetAButton());
+  // need to call pressed and released to clear flags
+  joy.GetAButtonPressed();
+  joy.GetAButtonReleased();
+
+  joysim.SetAButton(true);
+  joysim.NotifyNewData();
+  ASSERT_TRUE(joy.GetAButton());
+  ASSERT_TRUE(joy.GetAButtonPressed());
+  ASSERT_FALSE(joy.GetAButtonReleased());
+
+  joysim.SetAButton(false);
+  joysim.NotifyNewData();
+  ASSERT_FALSE(joy.GetAButton());
+  ASSERT_FALSE(joy.GetAButtonPressed());
+  ASSERT_TRUE(joy.GetAButtonReleased());
+}
diff --git a/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp b/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp
new file mode 100644
index 0000000..00259d1
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp
@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <units/angle.h>
+
+#include "frc/controller/ControllerUtil.h"
+#include "gtest/gtest.h"
+
+TEST(ControllerUtilTest, GetModulusError) {
+  // Test symmetric range
+  EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError(170.0 + 360.0, -170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError(170.0, -170.0 + 360.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0, frc::GetModulusError(-170.0, 170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0,
+                  frc::GetModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0));
+  EXPECT_FLOAT_EQ(20.0,
+                  frc::GetModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0));
+
+  // Test range starting at zero
+  EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, 190.0, 0.0, 360.0));
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError(170.0 + 360.0, 190.0, 0.0, 360.0));
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError(170.0, 190.0 + 360.0, 0.0, 360.0));
+
+  // Test asymmetric range that doesn't start at zero
+  EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -170.0, 190.0));
+
+  // Test all supported types
+  EXPECT_FLOAT_EQ(-20.0,
+                  frc::GetModulusError<double>(170.0, -170.0, -170.0, 190.0));
+  EXPECT_EQ(-20, frc::GetModulusError<int>(170, -170, -170, 190));
+  EXPECT_EQ(-20_deg, frc::GetModulusError<units::degree_t>(170_deg, -170_deg,
+                                                           -170_deg, 190_deg));
+}
diff --git a/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
new file mode 100644
index 0000000..fb72a4a
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <units/math.h>
+#include <units/time.h>
+#include <wpi/math>
+
+#include "frc/controller/HolonomicDriveController.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+static constexpr units::meter_t kTolerance{1 / 12.0};
+static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0};
+
+TEST(HolonomicDriveControllerTest, ReachesReference) {
+  frc::HolonomicDriveController controller{
+      frc2::PIDController{1.0, 0.0, 0.0}, frc2::PIDController{1.0, 0.0, 0.0},
+      frc::ProfiledPIDController<units::radian>{
+          1.0, 0.0, 0.0,
+          frc::TrapezoidProfile<units::radian>::Constraints{
+              6.28_rad_per_s, 3.14_rad_per_s / 1_s}}};
+
+  frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
+
+  auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
+                               frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      waypoints, {8.0_mps, 4.0_mps_sq});
+
+  constexpr auto kDt = 0.02_s;
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / kDt).to<double>(); ++i) {
+    auto state = trajectory.Sample(kDt * i);
+    auto [vx, vy, omega] = controller.Calculate(robotPose, state, 0_rad);
+
+    robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, vy * kDt, omega * kDt});
+  }
+
+  auto& endPose = trajectory.States().back().pose;
+  EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+  EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+  EXPECT_NEAR_UNITS(units::math::NormalizeAngle(robotPose.Rotation().Radians()),
+                    0_rad, kAngularTolerance);
+}
diff --git a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
index c68cb37..3580308 100644
--- a/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
+++ b/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2014-2020 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.                                                               */
@@ -21,7 +21,7 @@
   controller->SetP(1);
   controller->EnableContinuousInput(-180, 180);
 
-  EXPECT_TRUE(controller->Calculate(-179, 179) < 0);
+  EXPECT_LT(controller->Calculate(-179, 179), 0);
 }
 
 TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) {
diff --git a/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
new file mode 100644
index 0000000..6f9c36d
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <units/angle.h>
+#include <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+#include <wpi/math>
+
+#include "frc/controller/ProfiledPIDController.h"
+#include "gtest/gtest.h"
+
+class ProfiledPIDInputOutputTest : public testing::Test {
+ protected:
+  frc::ProfiledPIDController<units::degrees>* controller;
+
+  void SetUp() override {
+    controller = new frc::ProfiledPIDController<units::degrees>(
+        0, 0, 0, {360_deg_per_s, 180_deg_per_s_sq});
+  }
+
+  void TearDown() override { delete controller; }
+};
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-180_deg, 180_deg);
+
+  controller->Reset(-179_deg);
+  EXPECT_LT(controller->Calculate(-179_deg, 179_deg), 0);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest1) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-180_deg, 180_deg);
+
+  static constexpr units::degree_t kSetpoint{-179.0};
+  static constexpr units::degree_t kMeasurement{-179.0};
+  static constexpr units::degree_t kGoal{179.0};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            180_deg);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest2) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::math::pi},
+                                    units::radian_t{wpi::math::pi});
+
+  static constexpr units::radian_t kSetpoint{-3.4826633343199735};
+  static constexpr units::radian_t kMeasurement{-3.1352207333939606};
+  static constexpr units::radian_t kGoal{-3.534162788601621};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            units::radian_t{wpi::math::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest3) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::math::pi},
+                                    units::radian_t{wpi::math::pi});
+
+  static constexpr units::radian_t kSetpoint{-3.5176604690006377};
+  static constexpr units::radian_t kMeasurement{3.1191729343822456};
+  static constexpr units::radian_t kGoal{2.709680418117445};
+
+  controller->Reset(kSetpoint);
+  EXPECT_LT(controller->Calculate(kMeasurement, kGoal), 0.0);
+
+  // Error must be less than half the input range at all times
+  EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement),
+            units::radian_t{wpi::math::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ProportionalGainOutputTest) {
+  controller->SetP(4);
+
+  EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025_deg, 0_deg));
+}
+
+TEST_F(ProfiledPIDInputOutputTest, IntegralGainOutputTest) {
+  controller->SetI(4);
+
+  double out = 0;
+
+  for (int i = 0; i < 5; i++) {
+    out = controller->Calculate(0.025_deg, 0_deg);
+  }
+
+  EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, DerivativeGainOutputTest) {
+  controller->SetD(4);
+
+  controller->Calculate(0_deg, 0_deg);
+
+  EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+                   controller->Calculate(0.0025_deg, 0_deg));
+}
diff --git a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
index a600054..fb34385 100644
--- a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
+++ b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
@@ -1,11 +1,11 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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 <wpi/math>
+#include <units/math.h>
 
 #include "frc/controller/RamseteController.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
@@ -17,16 +17,6 @@
 static constexpr units::meter_t kTolerance{1 / 12.0};
 static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0};
 
-units::radian_t boundRadians(units::radian_t value) {
-  while (value > units::radian_t{wpi::math::pi}) {
-    value -= units::radian_t{wpi::math::pi * 2};
-  }
-  while (value <= units::radian_t{-wpi::math::pi}) {
-    value += units::radian_t{wpi::math::pi * 2};
-  }
-  return value;
-}
-
 TEST(RamseteControllerTest, ReachesReference) {
   frc::RamseteController controller{2.0, 0.7};
   frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};
@@ -47,11 +37,9 @@
   }
 
   auto& endPose = trajectory.States().back().pose;
-  EXPECT_NEAR_UNITS(endPose.Translation().X(), robotPose.Translation().X(),
-                    kTolerance);
-  EXPECT_NEAR_UNITS(endPose.Translation().Y(), robotPose.Translation().Y(),
-                    kTolerance);
-  EXPECT_NEAR_UNITS(boundRadians(endPose.Rotation().Radians() -
-                                 robotPose.Rotation().Radians()),
+  EXPECT_NEAR_UNITS(endPose.X(), robotPose.X(), kTolerance);
+  EXPECT_NEAR_UNITS(endPose.Y(), robotPose.Y(), kTolerance);
+  EXPECT_NEAR_UNITS(units::math::NormalizeAngle(endPose.Rotation().Radians() -
+                                                robotPose.Rotation().Radians()),
                     0_rad, kAngularTolerance);
 }
diff --git a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Pose2dTest.cpp
deleted file mode 100644
index 8b5f674..0000000
--- a/wpilibc/src/test/native/cpp/geometry/Pose2dTest.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 <cmath>
-
-#include "frc/geometry/Pose2d.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(Pose2dTest, TransformBy) {
-  const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
-  const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
-
-  const auto transformed = initial + transform;
-
-  EXPECT_NEAR(transformed.Translation().X().to<double>(),
-              1 + 5 / std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(transformed.Translation().Y().to<double>(),
-              2 + 5 / std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
-}
-
-TEST(Pose2dTest, RelativeTo) {
-  const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
-  const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
-
-  const auto finalRelativeToInitial = final.RelativeTo(initial);
-
-  EXPECT_NEAR(finalRelativeToInitial.Translation().X().to<double>(),
-              5.0 * std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(finalRelativeToInitial.Translation().Y().to<double>(), 0.0,
-              kEpsilon);
-  EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
-              kEpsilon);
-}
-
-TEST(Pose2dTest, Equality) {
-  const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
-  const Pose2d b{0_m, 5_m, Rotation2d(43_deg)};
-  EXPECT_TRUE(a == b);
-}
-
-TEST(Pose2dTest, Inequality) {
-  const Pose2d a{0_m, 5_m, Rotation2d(43_deg)};
-  const Pose2d b{0_m, 5_ft, Rotation2d(43_deg)};
-  EXPECT_TRUE(a != b);
-}
-
-TEST(Pose2dTest, Minus) {
-  const Pose2d initial{0_m, 0_m, Rotation2d(45.0_deg)};
-  const Pose2d final{5_m, 5_m, Rotation2d(45.0_deg)};
-
-  const auto transform = final - initial;
-
-  EXPECT_NEAR(transform.Translation().X().to<double>(), 5.0 * std::sqrt(2.0),
-              kEpsilon);
-  EXPECT_NEAR(transform.Translation().Y().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
-}
diff --git a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp
deleted file mode 100644
index ba80787..0000000
--- a/wpilibc/src/test/native/cpp/geometry/Rotation2dTest.cpp
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <cmath>
-
-#include <wpi/math>
-
-#include "frc/geometry/Rotation2d.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(Rotation2dTest, RadiansToDegrees) {
-  const Rotation2d one{units::radian_t(wpi::math::pi / 3)};
-  const Rotation2d two{units::radian_t(wpi::math::pi / 4)};
-
-  EXPECT_NEAR(one.Degrees().to<double>(), 60.0, kEpsilon);
-  EXPECT_NEAR(two.Degrees().to<double>(), 45.0, kEpsilon);
-}
-
-TEST(Rotation2dTest, DegreesToRadians) {
-  const auto one = Rotation2d(45.0_deg);
-  const auto two = Rotation2d(30.0_deg);
-
-  EXPECT_NEAR(one.Radians().to<double>(), wpi::math::pi / 4.0, kEpsilon);
-  EXPECT_NEAR(two.Radians().to<double>(), wpi::math::pi / 6.0, kEpsilon);
-}
-
-TEST(Rotation2dTest, RotateByFromZero) {
-  const Rotation2d zero;
-  auto sum = zero + Rotation2d(90.0_deg);
-
-  EXPECT_NEAR(sum.Radians().to<double>(), wpi::math::pi / 2.0, kEpsilon);
-  EXPECT_NEAR(sum.Degrees().to<double>(), 90.0, kEpsilon);
-}
-
-TEST(Rotation2dTest, RotateByNonZero) {
-  auto rot = Rotation2d(90.0_deg);
-  rot += Rotation2d(30.0_deg);
-
-  EXPECT_NEAR(rot.Degrees().to<double>(), 120.0, kEpsilon);
-}
-
-TEST(Rotation2dTest, Minus) {
-  const auto one = Rotation2d(70.0_deg);
-  const auto two = Rotation2d(30.0_deg);
-
-  EXPECT_NEAR((one - two).Degrees().to<double>(), 40.0, kEpsilon);
-}
-
-TEST(Rotation2dTest, Equality) {
-  const auto one = Rotation2d(43_deg);
-  const auto two = Rotation2d(43_deg);
-  EXPECT_TRUE(one == two);
-}
-
-TEST(Rotation2dTest, Inequality) {
-  const auto one = Rotation2d(43_deg);
-  const auto two = Rotation2d(43.5_deg);
-  EXPECT_TRUE(one != two);
-}
diff --git a/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp
deleted file mode 100644
index 99fced7..0000000
--- a/wpilibc/src/test/native/cpp/geometry/Translation2dTest.cpp
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <cmath>
-
-#include "frc/geometry/Translation2d.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(Translation2dTest, Sum) {
-  const Translation2d one{1.0_m, 3.0_m};
-  const Translation2d two{2.0_m, 5.0_m};
-
-  const auto sum = one + two;
-
-  EXPECT_NEAR(sum.X().to<double>(), 3.0, kEpsilon);
-  EXPECT_NEAR(sum.Y().to<double>(), 8.0, kEpsilon);
-}
-
-TEST(Translation2dTest, Difference) {
-  const Translation2d one{1.0_m, 3.0_m};
-  const Translation2d two{2.0_m, 5.0_m};
-
-  const auto difference = one - two;
-
-  EXPECT_NEAR(difference.X().to<double>(), -1.0, kEpsilon);
-  EXPECT_NEAR(difference.Y().to<double>(), -2.0, kEpsilon);
-}
-
-TEST(Translation2dTest, RotateBy) {
-  const Translation2d another{3.0_m, 0.0_m};
-  const auto rotated = another.RotateBy(Rotation2d(90.0_deg));
-
-  EXPECT_NEAR(rotated.X().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(rotated.Y().to<double>(), 3.0, kEpsilon);
-}
-
-TEST(Translation2dTest, Multiplication) {
-  const Translation2d original{3.0_m, 5.0_m};
-  const auto mult = original * 3;
-
-  EXPECT_NEAR(mult.X().to<double>(), 9.0, kEpsilon);
-  EXPECT_NEAR(mult.Y().to<double>(), 15.0, kEpsilon);
-}
-
-TEST(Translation2d, Division) {
-  const Translation2d original{3.0_m, 5.0_m};
-  const auto div = original / 2;
-
-  EXPECT_NEAR(div.X().to<double>(), 1.5, kEpsilon);
-  EXPECT_NEAR(div.Y().to<double>(), 2.5, kEpsilon);
-}
-
-TEST(Translation2dTest, Norm) {
-  const Translation2d one{3.0_m, 5.0_m};
-  EXPECT_NEAR(one.Norm().to<double>(), std::hypot(3, 5), kEpsilon);
-}
-
-TEST(Translation2dTest, Distance) {
-  const Translation2d one{1_m, 1_m};
-  const Translation2d two{6_m, 6_m};
-  EXPECT_NEAR(one.Distance(two).to<double>(), 5 * std::sqrt(2), kEpsilon);
-}
-
-TEST(Translation2dTest, UnaryMinus) {
-  const Translation2d original{-4.5_m, 7_m};
-  const auto inverted = -original;
-
-  EXPECT_NEAR(inverted.X().to<double>(), 4.5, kEpsilon);
-  EXPECT_NEAR(inverted.Y().to<double>(), -7, kEpsilon);
-}
-
-TEST(Translation2dTest, Equality) {
-  const Translation2d one{9_m, 5.5_m};
-  const Translation2d two{9_m, 5.5_m};
-  EXPECT_TRUE(one == two);
-}
-
-TEST(Translation2dTest, Inequality) {
-  const Translation2d one{9_m, 5.5_m};
-  const Translation2d two{9_m, 5.7_m};
-  EXPECT_TRUE(one != two);
-}
diff --git a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpilibc/src/test/native/cpp/geometry/Twist2dTest.cpp
deleted file mode 100644
index faf36d9..0000000
--- a/wpilibc/src/test/native/cpp/geometry/Twist2dTest.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 <cmath>
-
-#include <wpi/math>
-
-#include "frc/geometry/Pose2d.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(Twist2dTest, Straight) {
-  const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
-  const auto straightPose = Pose2d().Exp(straight);
-
-  EXPECT_NEAR(straightPose.Translation().X().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(straightPose.Translation().Y().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
-}
-
-TEST(Twist2dTest, QuarterCircle) {
-  const Twist2d quarterCircle{5.0_m / 2.0 * wpi::math::pi, 0_m,
-                              units::radian_t(wpi::math::pi / 2.0)};
-  const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
-
-  EXPECT_NEAR(quarterCirclePose.Translation().X().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(quarterCirclePose.Translation().Y().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
-              kEpsilon);
-}
-
-TEST(Twist2dTest, DiagonalNoDtheta) {
-  const Twist2d diagonal{2.0_m, 2.0_m, 0.0_deg};
-  const auto diagonalPose = Pose2d().Exp(diagonal);
-
-  EXPECT_NEAR(diagonalPose.Translation().X().to<double>(), 2.0, kEpsilon);
-  EXPECT_NEAR(diagonalPose.Translation().Y().to<double>(), 2.0, kEpsilon);
-  EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
-}
-
-TEST(Twist2dTest, Equality) {
-  const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
-  const Twist2d two{5.0_m, 1.0_m, 3.0_rad};
-  EXPECT_TRUE(one == two);
-}
-
-TEST(Twist2dTest, Inequality) {
-  const Twist2d one{5.0_m, 1.0_m, 3.0_rad};
-  const Twist2d two{5.0_m, 1.2_m, 3.0_rad};
-  EXPECT_TRUE(one != two);
-}
-
-TEST(Twist2dTest, Pose2dLog) {
-  const Pose2d end{5_m, 5_m, Rotation2d(90_deg)};
-  const Pose2d start{};
-
-  const auto twist = start.Log(end);
-
-  EXPECT_NEAR(twist.dx.to<double>(), 5 / 2.0 * wpi::math::pi, kEpsilon);
-  EXPECT_NEAR(twist.dy.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(twist.dtheta.to<double>(), wpi::math::pi / 2.0, kEpsilon);
-}
diff --git a/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
deleted file mode 100644
index 3fb63fb..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/ChassisSpeedsTest.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 "frc/kinematics/ChassisSpeeds.h"
-#include "gtest/gtest.h"
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(ChassisSpeeds, FieldRelativeConstruction) {
-  const auto chassisSpeeds = frc::ChassisSpeeds::FromFieldRelativeSpeeds(
-      1.0_mps, 0.0_mps, 0.5_rad_per_s, frc::Rotation2d(-90.0_deg));
-
-  EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), kEpsilon);
-  EXPECT_NEAR(1.0, chassisSpeeds.vy.to<double>(), kEpsilon);
-  EXPECT_NEAR(0.5, chassisSpeeds.omega.to<double>(), kEpsilon);
-}
diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
deleted file mode 100644
index ad0d3c7..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
+++ /dev/null
@@ -1,77 +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 <units/units.h>
-#include <wpi/math>
-
-#include "frc/kinematics/ChassisSpeeds.h"
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 1E-9;
-
-TEST(DifferentialDriveKinematics, InverseKinematicsFromZero) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const ChassisSpeeds chassisSpeeds;
-  const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
-
-  EXPECT_NEAR(wheelSpeeds.left.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(wheelSpeeds.right.to<double>(), 0, kEpsilon);
-}
-
-TEST(DifferentialDriveKinematics, ForwardKinematicsFromZero) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const DifferentialDriveWheelSpeeds wheelSpeeds;
-  const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
-}
-
-TEST(DifferentialDriveKinematics, InverseKinematicsForStraightLine) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const ChassisSpeeds chassisSpeeds{3.0_mps, 0_mps, 0_rad_per_s};
-  const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
-
-  EXPECT_NEAR(wheelSpeeds.left.to<double>(), 3, kEpsilon);
-  EXPECT_NEAR(wheelSpeeds.right.to<double>(), 3, kEpsilon);
-}
-
-TEST(DifferentialDriveKinematics, ForwardKinematicsForStraightLine) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const DifferentialDriveWheelSpeeds wheelSpeeds{3.0_mps, 3.0_mps};
-  const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 3, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0, kEpsilon);
-}
-
-TEST(DifferentialDriveKinematics, InverseKinematicsForRotateInPlace) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const ChassisSpeeds chassisSpeeds{0.0_mps, 0.0_mps,
-                                    units::radians_per_second_t{wpi::math::pi}};
-  const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds);
-
-  EXPECT_NEAR(wheelSpeeds.left.to<double>(), -0.381 * wpi::math::pi, kEpsilon);
-  EXPECT_NEAR(wheelSpeeds.right.to<double>(), +0.381 * wpi::math::pi, kEpsilon);
-}
-
-TEST(DifferentialDriveKinematics, ForwardKinematicsForRotateInPlace) {
-  const DifferentialDriveKinematics kinematics{0.381_m * 2};
-  const DifferentialDriveWheelSpeeds wheelSpeeds{
-      units::meters_per_second_t(+0.381 * wpi::math::pi),
-      units::meters_per_second_t(-0.381 * wpi::math::pi)};
-  const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), -wpi::math::pi, kEpsilon);
-}
diff --git a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
deleted file mode 100644
index 8acaf92..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.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 <wpi/math>
-
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/kinematics/DifferentialDriveOdometry.h"
-#include "gtest/gtest.h"
-
-static constexpr double kEpsilon = 1E-9;
-
-using namespace frc;
-
-TEST(DifferentialDriveOdometry, EncoderDistances) {
-  DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
-
-  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);
-  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
-}
diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
deleted file mode 100644
index 75f395d..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
+++ /dev/null
@@ -1,230 +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 <units/units.h>
-#include <wpi/math>
-
-#include "frc/geometry/Translation2d.h"
-#include "frc/kinematics/MecanumDriveKinematics.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-class MecanumDriveKinematicsTest : public ::testing::Test {
- protected:
-  Translation2d m_fl{12_m, 12_m};
-  Translation2d m_fr{12_m, -12_m};
-  Translation2d m_bl{-12_m, 12_m};
-  Translation2d m_br{-12_m, -12_m};
-
-  MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
-};
-
-TEST_F(MecanumDriveKinematicsTest, StraightLineInverseKinematics) {
-  ChassisSpeeds speeds{5_mps, 0_mps, 0_rad_per_s};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
-
-  /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534
-  */
-
-  EXPECT_NEAR(3.536, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(3.536, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(3.536, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(3.536, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
-                                      3.536_mps};
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-  By equation (13.13) of the state-space-guide, the chassis motion from wheel
-  velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
-  [[5][0][0]]
-  */
-
-  EXPECT_NEAR(5.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, StrafeInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 4_mps, 0_rad_per_s};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
-
-  /*
-  By equation (13.12) of the state-space-guide, the wheel speeds should
-  be as follows:
-  velocities: fl -2.828427 fr 2.828427 rl 2.828427 rr -2.828427
-  */
-
-  EXPECT_NEAR(-2.828427, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(2.828427, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(2.828427, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(-2.828427, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{-2.828427_mps, 2.828427_mps, 2.828427_mps,
-                                      -2.828427_mps};
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-    By equation (13.13) of the state-space-guide, the chassis motion from wheel
-    velocities: fl 3.535534 fr 3.535534 rl 3.535534 rr 3.535534 will be
-    [[5][0][0]]
-  */
-
-  EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(4.0, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(0.0, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::math::pi)};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
-
-  /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191
-  */
-
-  EXPECT_NEAR(-106.62919, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(106.62919, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(-106.62919, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(106.62919, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{-106.62919_mps, 106.62919_mps,
-                                      -106.62919_mps, 106.62919_mps};
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-    By equation (13.13) of the state-space-guide, the chassis motion from wheel
-    velocities: fl -106.629191 fr 106.629191 rl -106.629191 rr 106.629191 should
-    be [[0][0][2pi]]
-  */
-
-  EXPECT_NEAR(0.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(0.0, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) {
-  ChassisSpeeds speeds{2_mps, 3_mps, 1_rad_per_s};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds);
-
-  /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456
-  */
-
-  EXPECT_NEAR(-17.677670, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(20.506097, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(-13.435, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(16.26, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{-17.677670_mps, 20.506097_mps,
-                                      -13.435_mps, 16.26_mps};
-
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-    By equation (13.13) of the state-space-guide, the chassis motion from wheel
-    velocities: fl -17.677670 fr 20.506097 rl -13.435029 rr 16.263456 should be
-    [[2][3][1]]
-  */
-
-  EXPECT_NEAR(2.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(3.0, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, OffCenterRotationInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 0_mps, 1_rad_per_s};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
-
-  /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl 0.000000 fr 16.970563 rl -16.970563 rr 33.941125
-  */
-
-  EXPECT_NEAR(0, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(16.971, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(-16.971, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(33.941, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{0_mps, 16.971_mps, -16.971_mps,
-                                      33.941_mps};
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-    By equation (13.13) of the state-space-guide, the chassis motion from the
-    wheel velocities should be [[12][-12][1]]
-  */
-
-  EXPECT_NEAR(12.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(-12, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest,
-       OffCenterTranslationRotationInverseKinematics) {
-  ChassisSpeeds speeds{5_mps, 2_mps, 1_rad_per_s};
-  auto moduleStates = kinematics.ToWheelSpeeds(speeds, m_fl);
-
-  /*
-    By equation (13.12) of the state-space-guide, the wheel speeds should
-    be as follows:
-    velocities: fl 2.121320 fr 21.920310 rl -12.020815 rr 36.062446
-  */
-  EXPECT_NEAR(2.12, moduleStates.frontLeft.to<double>(), 0.1);
-  EXPECT_NEAR(21.92, moduleStates.frontRight.to<double>(), 0.1);
-  EXPECT_NEAR(-12.02, moduleStates.rearLeft.to<double>(), 0.1);
-  EXPECT_NEAR(36.06, moduleStates.rearRight.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest,
-       OffCenterTranslationRotationForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{2.12_mps, 21.92_mps, -12.02_mps,
-                                      36.06_mps};
-  auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds);
-
-  /*
-    By equation (13.13) of the state-space-guide, the chassis motion from the
-    wheel velocities should be [[17][-10][1]]
-  */
-
-  EXPECT_NEAR(17.0, chassisSpeeds.vx.to<double>(), 0.1);
-  EXPECT_NEAR(-10, chassisSpeeds.vy.to<double>(), 0.1);
-  EXPECT_NEAR(1.0, chassisSpeeds.omega.to<double>(), 0.1);
-}
-
-TEST_F(MecanumDriveKinematicsTest, NormalizeTest) {
-  MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 6_mps, 4_mps, 7_mps};
-  wheelSpeeds.Normalize(5.5_mps);
-
-  double kFactor = 5.5 / 7.0;
-
-  EXPECT_NEAR(wheelSpeeds.frontLeft.to<double>(), 5.0 * kFactor, 1E-9);
-  EXPECT_NEAR(wheelSpeeds.frontRight.to<double>(), 6.0 * kFactor, 1E-9);
-  EXPECT_NEAR(wheelSpeeds.rearLeft.to<double>(), 4.0 * kFactor, 1E-9);
-  EXPECT_NEAR(wheelSpeeds.rearRight.to<double>(), 7.0 * kFactor, 1E-9);
-}
diff --git a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
deleted file mode 100644
index 5d990bf..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ /dev/null
@@ -1,72 +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/MecanumDriveOdometry.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-class MecanumDriveOdometryTest : public ::testing::Test {
- protected:
-  Translation2d m_fl{12_m, 12_m};
-  Translation2d m_fr{12_m, -12_m};
-  Translation2d m_bl{-12_m, 12_m};
-  Translation2d m_br{-12_m, -12_m};
-
-  MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br};
-  MecanumDriveOdometry odometry{kinematics, 0_rad};
-};
-
-TEST_F(MecanumDriveOdometryTest, MultipleConsecutiveUpdates) {
-  odometry.ResetPosition(Pose2d(), 0_rad);
-  MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
-                                      3.536_mps};
-
-  odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
-  auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
-
-  EXPECT_NEAR(secondPose.Translation().X().to<double>(), 0.0, 0.01);
-  EXPECT_NEAR(secondPose.Translation().Y().to<double>(), 0.0, 0.01);
-  EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
-}
-
-TEST_F(MecanumDriveOdometryTest, TwoIterations) {
-  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{});
-  auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), 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);
-}
-
-TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
-  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{});
-  auto pose = odometry.UpdateWithTime(1_s, Rotation2d(90_deg), speeds);
-
-  EXPECT_NEAR(pose.Translation().X().to<double>(), 12, 0.01);
-  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/SwerveDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
deleted file mode 100644
index 8fd67ea..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ /dev/null
@@ -1,183 +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 <units/units.h>
-#include <wpi/math>
-
-#include "frc/geometry/Translation2d.h"
-#include "frc/kinematics/SwerveDriveKinematics.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 0.1;
-
-class SwerveDriveKinematicsTest : public ::testing::Test {
- protected:
-  Translation2d m_fl{12_m, 12_m};
-  Translation2d m_fr{12_m, -12_m};
-  Translation2d m_bl{-12_m, 12_m};
-  Translation2d m_br{-12_m, -12_m};
-
-  SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br};
-};
-
-TEST_F(SwerveDriveKinematicsTest, StraightLineInverseKinematics) {
-  ChassisSpeeds speeds{5.0_mps, 0.0_mps, 0.0_rad_per_s};
-
-  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
-
-  EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
-
-  EXPECT_NEAR(fl.angle.Radians().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(fr.angle.Radians().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(bl.angle.Radians().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(br.angle.Radians().to<double>(), 0.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
-  SwerveModuleState state{5.0_mps, Rotation2d()};
-
-  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, StraightStrafeInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 5_mps, 0_rad_per_s};
-  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
-
-  EXPECT_NEAR(fl.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(fr.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(bl.speed.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(br.speed.to<double>(), 5.0, kEpsilon);
-
-  EXPECT_NEAR(fl.angle.Degrees().to<double>(), 90.0, kEpsilon);
-  EXPECT_NEAR(fr.angle.Degrees().to<double>(), 90.0, kEpsilon);
-  EXPECT_NEAR(bl.angle.Degrees().to<double>(), 90.0, kEpsilon);
-  EXPECT_NEAR(br.angle.Degrees().to<double>(), 90.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) {
-  SwerveModuleState state{5_mps, Rotation2d(90_deg)};
-  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(state, state, state, state);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 0.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::math::pi)};
-  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds);
-
-  EXPECT_NEAR(fl.speed.to<double>(), 106.63, kEpsilon);
-  EXPECT_NEAR(fr.speed.to<double>(), 106.63, kEpsilon);
-  EXPECT_NEAR(bl.speed.to<double>(), 106.63, kEpsilon);
-  EXPECT_NEAR(br.speed.to<double>(), 106.63, kEpsilon);
-
-  EXPECT_NEAR(fl.angle.Degrees().to<double>(), 135.0, kEpsilon);
-  EXPECT_NEAR(fr.angle.Degrees().to<double>(), 45.0, kEpsilon);
-  EXPECT_NEAR(bl.angle.Degrees().to<double>(), -135.0, kEpsilon);
-  EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
-  SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
-  SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};
-  SwerveModuleState bl{106.629_mps, Rotation2d(-135_deg)};
-  SwerveModuleState br{106.629_mps, Rotation2d(-45_deg)};
-
-  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::math::pi)};
-  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl);
-
-  EXPECT_NEAR(fl.speed.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(fr.speed.to<double>(), 150.796, kEpsilon);
-  EXPECT_NEAR(bl.speed.to<double>(), 150.796, kEpsilon);
-  EXPECT_NEAR(br.speed.to<double>(), 213.258, kEpsilon);
-
-  EXPECT_NEAR(fl.angle.Degrees().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(fr.angle.Degrees().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(bl.angle.Degrees().to<double>(), -90.0, kEpsilon);
-  EXPECT_NEAR(br.angle.Degrees().to<double>(), -45.0, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
-  SwerveModuleState fl{0.0_mps, Rotation2d(0_deg)};
-  SwerveModuleState fr{150.796_mps, Rotation2d(0_deg)};
-  SwerveModuleState bl{150.796_mps, Rotation2d(-90_deg)};
-  SwerveModuleState br{213.258_mps, Rotation2d(-45_deg)};
-
-  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 75.398, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -75.398, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 2 * wpi::math::pi, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest,
-       OffCenterCORRotationAndTranslationInverseKinematics) {
-  ChassisSpeeds speeds{0_mps, 3.0_mps, 1.5_rad_per_s};
-  auto [fl, fr, bl, br] =
-      m_kinematics.ToSwerveModuleStates(speeds, Translation2d(24_m, 0_m));
-
-  EXPECT_NEAR(fl.speed.to<double>(), 23.43, kEpsilon);
-  EXPECT_NEAR(fr.speed.to<double>(), 23.43, kEpsilon);
-  EXPECT_NEAR(bl.speed.to<double>(), 54.08, kEpsilon);
-  EXPECT_NEAR(br.speed.to<double>(), 54.08, kEpsilon);
-
-  EXPECT_NEAR(fl.angle.Degrees().to<double>(), -140.19, kEpsilon);
-  EXPECT_NEAR(fr.angle.Degrees().to<double>(), -39.81, kEpsilon);
-  EXPECT_NEAR(bl.angle.Degrees().to<double>(), -109.44, kEpsilon);
-  EXPECT_NEAR(br.angle.Degrees().to<double>(), -70.56, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest,
-       OffCenterCORRotationAndTranslationForwardKinematics) {
-  SwerveModuleState fl{23.43_mps, Rotation2d(-140.19_deg)};
-  SwerveModuleState fr{23.43_mps, Rotation2d(-39.81_deg)};
-  SwerveModuleState bl{54.08_mps, Rotation2d(-109.44_deg)};
-  SwerveModuleState br{54.08_mps, Rotation2d(-70.56_deg)};
-
-  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(fl, fr, bl, br);
-
-  EXPECT_NEAR(chassisSpeeds.vx.to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.vy.to<double>(), -33.0, kEpsilon);
-  EXPECT_NEAR(chassisSpeeds.omega.to<double>(), 1.5, kEpsilon);
-}
-
-TEST_F(SwerveDriveKinematicsTest, NormalizeTest) {
-  SwerveModuleState state1{5.0_mps, Rotation2d()};
-  SwerveModuleState state2{6.0_mps, Rotation2d()};
-  SwerveModuleState state3{4.0_mps, Rotation2d()};
-  SwerveModuleState state4{7.0_mps, Rotation2d()};
-
-  std::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
-  SwerveDriveKinematics<4>::NormalizeWheelSpeeds(&arr, 5.5_mps);
-
-  double kFactor = 5.5 / 7.0;
-
-  EXPECT_NEAR(arr[0].speed.to<double>(), 5.0 * kFactor, kEpsilon);
-  EXPECT_NEAR(arr[1].speed.to<double>(), 6.0 * kFactor, kEpsilon);
-  EXPECT_NEAR(arr[2].speed.to<double>(), 4.0 * kFactor, kEpsilon);
-  EXPECT_NEAR(arr[3].speed.to<double>(), 7.0 * kFactor, kEpsilon);
-}
diff --git a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
deleted file mode 100644
index d0399dc..0000000
--- a/wpilibc/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ /dev/null
@@ -1,74 +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/SwerveDriveKinematics.h"
-#include "frc/kinematics/SwerveDriveOdometry.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-static constexpr double kEpsilon = 0.01;
-
-class SwerveDriveOdometryTest : public ::testing::Test {
- protected:
-  Translation2d m_fl{12_m, 12_m};
-  Translation2d m_fr{12_m, -12_m};
-  Translation2d m_bl{-12_m, 12_m};
-  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, 0_rad};
-};
-
-TEST_F(SwerveDriveOdometryTest, TwoIterations) {
-  SwerveModuleState state{5_mps, Rotation2d()};
-
-  m_odometry.ResetPosition(Pose2d(), 0_rad);
-  m_odometry.UpdateWithTime(0_s, Rotation2d(), SwerveModuleState(),
-                            SwerveModuleState(), SwerveModuleState(),
-                            SwerveModuleState());
-  auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), 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);
-}
-
-TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
-  SwerveModuleState fl{18.85_mps, Rotation2d(90_deg)};
-  SwerveModuleState fr{42.15_mps, Rotation2d(26.565_deg)};
-  SwerveModuleState bl{18.85_mps, Rotation2d(-90_deg)};
-  SwerveModuleState br{42.15_mps, Rotation2d(-26.565_deg)};
-
-  SwerveModuleState zero{0_mps, Rotation2d()};
-
-  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);
-
-  EXPECT_NEAR(12.0, pose.Translation().X().to<double>(), kEpsilon);
-  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/simulation/AccelerometerSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/AccelerometerSimTest.cpp
new file mode 100644
index 0000000..d2c7ea8
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/AccelerometerSimTest.cpp
@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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/simulation/BuiltInAccelerometerSim.h"  // NOLINT(build/include_order)
+
+#include <hal/Accelerometer.h>
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+
+using namespace frc::sim;
+
+TEST(AcclerometerSimTests, TestActiveCallback) {
+  HAL_Initialize(500, 0);
+
+  BuiltInAccelerometerSim sim;
+
+  sim.ResetData();
+
+  bool wasTriggered = false;
+  bool lastValue = false;
+
+  auto cb = sim.RegisterActiveCallback(
+      [&](wpi::StringRef name, const HAL_Value* value) {
+        wasTriggered = true;
+        lastValue = value->data.v_boolean;
+      },
+      false);
+
+  EXPECT_FALSE(wasTriggered);
+
+  HAL_SetAccelerometerActive(true);
+
+  EXPECT_TRUE(wasTriggered);
+  EXPECT_TRUE(lastValue);
+}
diff --git a/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp
new file mode 100644
index 0000000..f32be7e
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp
@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <units/math.h>
+#include <wpi/math>
+
+#include "frc/AnalogEncoder.h"
+#include "frc/AnalogInput.h"
+#include "frc/simulation/AnalogEncoderSim.h"
+#include "gtest/gtest.h"
+
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
+
+TEST(AnalogEncoderSimTest, TestBasic) {
+  frc::AnalogInput ai(0);
+  frc::AnalogEncoder encoder{ai};
+  frc::sim::AnalogEncoderSim encoderSim{encoder};
+
+  encoderSim.SetPosition(180_deg);
+  EXPECT_NEAR(encoder.Get().to<double>(), 0.5, 1E-8);
+  EXPECT_NEAR(encoderSim.GetTurns().to<double>(), 0.5, 1E-8);
+  EXPECT_NEAR(encoderSim.GetPosition().Radians().to<double>(), wpi::math::pi,
+              1E-8);
+}
diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
new file mode 100644
index 0000000..f09344e
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
@@ -0,0 +1,115 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <units/current.h>
+#include <units/math.h>
+#include <units/moment_of_inertia.h>
+
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/controller/RamseteController.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/simulation/DifferentialDrivetrainSim.h"
+#include "frc/system/RungeKutta.h"
+#include "frc/system/plant/DCMotor.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+#include "gtest/gtest.h"
+
+TEST(DifferentialDriveSim, Convergence) {
+  auto motor = frc::DCMotor::NEO(2);
+  auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
+      motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
+
+  frc::DifferentialDriveKinematics kinematics{24_in};
+  frc::sim::DifferentialDrivetrainSim sim{plant, 24_in, motor, 1.0, 2_in};
+
+  frc::LinearPlantInversionFeedforward feedforward{plant, 20_ms};
+  frc::RamseteController ramsete;
+
+  feedforward.Reset(frc::MakeMatrix<2, 1>(0.0, 0.0));
+
+  // Ground truth.
+  Eigen::Matrix<double, 7, 1> groundTruthX =
+      Eigen::Matrix<double, 7, 1>::Zero();
+
+  frc::TrajectoryConfig config{1_mps, 1_mps_sq};
+  config.AddConstraint(
+      frc::DifferentialDriveKinematicsConstraint(kinematics, 1_mps));
+
+  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      frc::Pose2d(), {}, frc::Pose2d(2_m, 2_m, 0_rad), config);
+
+  for (double t = 0; t < trajectory.TotalTime().to<double>(); t += 0.02) {
+    auto state = trajectory.Sample(20_ms);
+    auto ramseteOut = ramsete.Calculate(sim.GetPose(), state);
+
+    auto [l, r] = kinematics.ToWheelSpeeds(ramseteOut);
+    auto voltages = feedforward.Calculate(
+        frc::MakeMatrix<2, 1>(l.to<double>(), r.to<double>()));
+
+    // Sim periodic code.
+    sim.SetInputs(units::volt_t(voltages(0, 0)), units::volt_t(voltages(1, 0)));
+    sim.Update(20_ms);
+
+    // Update ground truth.
+    groundTruthX = frc::RungeKutta(
+        [&sim](const auto& x, const auto& u) -> Eigen::Matrix<double, 7, 1> {
+          return sim.Dynamics(x, u);
+        },
+        groundTruthX, voltages, 20_ms);
+  }
+
+  EXPECT_NEAR(groundTruthX(0, 0), sim.GetState(0), 1E-6);
+  EXPECT_NEAR(groundTruthX(1, 0), sim.GetState(1), 1E-6);
+  EXPECT_NEAR(groundTruthX(2, 0), sim.GetState(2), 1E-6);
+}
+
+TEST(DifferentialDriveSim, Current) {
+  auto motor = frc::DCMotor::NEO(2);
+  auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
+      motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0);
+
+  frc::DifferentialDriveKinematics kinematics{24_in};
+  frc::sim::DifferentialDrivetrainSim sim{plant, 24_in, motor, 1.0, 2_in};
+
+  sim.SetInputs(-12_V, 12_V);
+  for (int i = 0; i < 10; ++i) {
+    sim.Update(20_ms);
+  }
+  EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
+
+  sim.SetInputs(12_V, 12_V);
+  for (int i = 0; i < 20; ++i) {
+    sim.Update(20_ms);
+  }
+  EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
+
+  sim.SetInputs(-12_V, 12_V);
+  for (int i = 0; i < 30; ++i) {
+    sim.Update(20_ms);
+  }
+  EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
+}
+
+TEST(DifferentialDriveSim, ModelStability) {
+  auto motor = frc::DCMotor::NEO(2);
+  auto plant = frc::LinearSystemId::DrivetrainVelocitySystem(
+      motor, 50_kg, 2_in, 12_in, 2_kg_sq_m, 5.0);
+
+  frc::DifferentialDriveKinematics kinematics{24_in};
+  frc::sim::DifferentialDrivetrainSim sim{plant, 24_in, motor, 1.0, 2_in};
+
+  sim.SetInputs(2_V, 4_V);
+
+  // 10 seconds should be enough time to verify stability
+  for (int i = 0; i < 500; ++i) {
+    sim.Update(20_ms);
+  }
+
+  EXPECT_LT(units::math::abs(sim.GetPose().Translation().Norm()), 100_m);
+}
diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
new file mode 100644
index 0000000..05b0dc9
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <units/time.h>
+
+#include "frc/Encoder.h"
+#include "frc/PWMVictorSPX.h"
+#include "frc/RobotController.h"
+#include "frc/StateSpaceUtil.h"
+#include "frc/controller/PIDController.h"
+#include "frc/simulation/ElevatorSim.h"
+#include "frc/simulation/EncoderSim.h"
+#include "frc/system/plant/DCMotor.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "gtest/gtest.h"
+
+TEST(ElevatorSim, StateSpaceSim) {
+  frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg,
+                            units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 3_m,
+                            {0.01});
+  frc2::PIDController controller(10, 0.0, 0.0);
+
+  frc::PWMVictorSPX motor(0);
+  frc::Encoder encoder(0, 1);
+  frc::sim::EncoderSim encoderSim(encoder);
+
+  for (size_t i = 0; i < 100; ++i) {
+    controller.SetSetpoint(2.0);
+    auto nextVoltage = controller.Calculate(encoderSim.GetDistance());
+    motor.Set(nextVoltage / frc::RobotController::GetInputVoltage());
+
+    auto u = frc::MakeMatrix<1, 1>(motor.Get() *
+                                   frc::RobotController::GetInputVoltage());
+    sim.SetInput(u);
+    sim.Update(20_ms);
+
+    const auto& y = sim.GetOutput();
+    encoderSim.SetDistance(y(0));
+  }
+
+  EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().to<double>(), 0.2);
+}
+
+TEST(ElevatorSim, MinMax) {
+  frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg,
+                            units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 1_m,
+                            {0.01});
+  for (size_t i = 0; i < 100; ++i) {
+    sim.SetInput(frc::MakeMatrix<1, 1>(0.0));
+    sim.Update(20_ms);
+
+    auto height = sim.GetPosition();
+    EXPECT_TRUE(height > -0.05_m);
+  }
+
+  for (size_t i = 0; i < 100; ++i) {
+    sim.SetInput(frc::MakeMatrix<1, 1>(12.0));
+    sim.Update(20_ms);
+
+    auto height = sim.GetPosition();
+    EXPECT_TRUE(height < 1.05_m);
+  }
+}
diff --git a/wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp
new file mode 100644
index 0000000..ea7b809
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp
@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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/SimDevice.h>
+#include <wpi/StringRef.h>
+
+#include "frc/simulation/SimDeviceSim.h"
+#include "gtest/gtest.h"
+
+using namespace frc::sim;
+
+TEST(SimDeviceSimTests, TestBasic) {
+  hal::SimDevice dev{"test"};
+  hal::SimBoolean devBool = dev.CreateBoolean("bool", false, false);
+
+  SimDeviceSim sim{"test"};
+  hal::SimBoolean simBool = sim.GetBoolean("bool");
+  EXPECT_FALSE(simBool.Get());
+  simBool.Set(true);
+  EXPECT_TRUE(devBool.Get());
+}
+
+TEST(SimDeviceSimTests, TestEnumerateDevices) {
+  hal::SimDevice dev{"test"};
+
+  bool foundit = false;
+  SimDeviceSim::EnumerateDevices(
+      "te", [&](const char* name, HAL_SimDeviceHandle handle) {
+        if (wpi::StringRef(name) == "test") foundit = true;
+      });
+  EXPECT_TRUE(foundit);
+}
diff --git a/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp b/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp
new file mode 100644
index 0000000..1b86e05
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2020 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 <exception>
+
+#include <hal/HAL.h>
+
+#include "frc/simulation/AddressableLEDSim.h"
+#include "frc/simulation/AnalogGyroSim.h"
+#include "frc/simulation/AnalogInputSim.h"
+#include "frc/simulation/AnalogOutputSim.h"
+#include "frc/simulation/AnalogTriggerSim.h"
+#include "frc/simulation/BuiltInAccelerometerSim.h"
+#include "frc/simulation/DIOSim.h"
+#include "frc/simulation/DigitalPWMSim.h"
+#include "frc/simulation/DriverStationSim.h"
+#include "frc/simulation/DutyCycleSim.h"
+#include "frc/simulation/EncoderSim.h"
+#include "frc/simulation/PCMSim.h"
+#include "frc/simulation/PDPSim.h"
+#include "frc/simulation/PWMSim.h"
+#include "frc/simulation/RelaySim.h"
+#include "frc/simulation/RoboRioSim.h"
+#include "frc/simulation/SPIAccelerometerSim.h"
+#include "gtest/gtest.h"
+
+using namespace frc::sim;
+
+TEST(SimInitializationTests, TestAllInitialize) {
+  HAL_Initialize(500, 0);
+  BuiltInAccelerometerSim biacsim;
+  AnalogGyroSim agsim{0};
+  AnalogInputSim aisim{0};
+  AnalogOutputSim aosim{0};
+  EXPECT_THROW(AnalogTriggerSim::CreateForChannel(0), std::out_of_range);
+  EXPECT_THROW(DigitalPWMSim::CreateForChannel(0), std::out_of_range);
+  DIOSim diosim{0};
+  DriverStationSim dssim;
+  (void)dssim;
+  EncoderSim esim = EncoderSim::CreateForIndex(0);
+  (void)esim;
+  PCMSim pcmsim{0};
+  PDPSim pdpsim{0};
+  PWMSim pwmsim{0};
+  RelaySim rsim{0};
+  RoboRioSim rrsim;
+  (void)rrsim;
+  SPIAccelerometerSim sasim{0};
+  DutyCycleSim dcsim = DutyCycleSim::CreateForIndex(0);
+  (void)dcsim;
+  AddressableLEDSim adLED;
+}
diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
new file mode 100644
index 0000000..a553985
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <wpi/math>
+
+#include "frc/simulation/SingleJointedArmSim.h"
+#include "gtest/gtest.h"
+
+TEST(SingleJointedArmTest, Disabled) {
+  frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 3_kg_sq_m,
+                                    9.5_in, -180_deg, 0_deg, 10_lb, true);
+  sim.SetState(frc::MakeMatrix<2, 1>(0.0, 0.0));
+
+  for (size_t i = 0; i < 12 / 0.02; ++i) {
+    sim.SetInput(frc::MakeMatrix<1, 1>(0.0));
+    sim.Update(20_ms);
+  }
+
+  // The arm should swing down.
+  EXPECT_NEAR(sim.GetAngle().to<double>(), -wpi::math::pi / 2, 0.01);
+}
diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
new file mode 100644
index 0000000..5766751
--- /dev/null
+++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 <units/angular_acceleration.h>
+#include <units/angular_velocity.h>
+
+#include "frc/Encoder.h"
+#include "frc/PWMVictorSPX.h"
+#include "frc/RobotController.h"
+#include "frc/controller/PIDController.h"
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "frc/simulation/BatterySim.h"
+#include "frc/simulation/DifferentialDrivetrainSim.h"
+#include "frc/simulation/ElevatorSim.h"
+#include "frc/simulation/EncoderSim.h"
+#include "frc/simulation/FlywheelSim.h"
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/simulation/PWMSim.h"
+#include "frc/simulation/RoboRioSim.h"
+#include "frc/simulation/SingleJointedArmSim.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "gtest/gtest.h"
+
+TEST(StateSpaceSimTest, TestFlywheelSim) {
+  const frc::LinearSystem<1, 1, 1> plant =
+      frc::LinearSystemId::IdentifyVelocitySystem<units::radian>(
+          0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq);
+  frc::sim::FlywheelSim sim{plant, frc::DCMotor::NEO(2), 1.0};
+  frc2::PIDController controller{0.2, 0.0, 0.0};
+  frc::SimpleMotorFeedforward<units::radian> feedforward{
+      0_V, 0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq};
+  frc::Encoder encoder{0, 1};
+  frc::sim::EncoderSim encoderSim{encoder};
+  frc::PWMVictorSPX motor{0};
+
+  for (int i = 0; i < 100; i++) {
+    // RobotPeriodic runs first
+    auto voltageOut = controller.Calculate(encoder.GetRate(), 200.0);
+    motor.SetVoltage(units::volt_t(voltageOut) +
+                     feedforward.Calculate(200_rad_per_s));
+
+    // Then, SimulationPeriodic runs
+    frc::sim::RoboRioSim::SetVInVoltage(
+        frc::sim::BatterySim::Calculate({sim.GetCurrentDraw()}));
+    sim.SetInput(frc::MakeMatrix<1, 1>(
+        motor.Get() * frc::RobotController::GetInputVoltage()));
+    sim.Update(20_ms);
+    encoderSim.SetRate(sim.GetAngularVelocity().to<double>());
+  }
+
+  ASSERT_TRUE(std::abs(200 - encoder.GetRate()) < 0.1);
+}
diff --git a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
deleted file mode 100644
index f2f0a38..0000000
--- a/wpilibc/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ /dev/null
@@ -1,133 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 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 <chrono>
-#include <iostream>
-#include <vector>
-
-#include <units/units.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/geometry/Rotation2d.h"
-#include "frc/spline/QuinticHermiteSpline.h"
-#include "frc/spline/SplineHelper.h"
-#include "frc/spline/SplineParameterizer.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-namespace frc {
-class CubicHermiteSplineTest : public ::testing::Test {
- protected:
-  static void Run(const Pose2d& a, const std::vector<Translation2d>& waypoints,
-                  const Pose2d& b) {
-    // Start the timer.
-    const auto start = std::chrono::high_resolution_clock::now();
-
-    // Generate and parameterize the spline.
-
-    const auto [startCV, endCV] =
-        SplineHelper::CubicControlVectorsFromWaypoints(a, waypoints, b);
-
-    const auto splines =
-        SplineHelper::CubicSplinesFromControlVectors(startCV, waypoints, endCV);
-    std::vector<Spline<3>::PoseWithCurvature> poses;
-
-    poses.push_back(splines[0].GetPoint(0.0));
-
-    for (auto&& spline : splines) {
-      auto x = SplineParameterizer::Parameterize(spline);
-      poses.insert(std::end(poses), std::begin(x) + 1, std::end(x));
-    }
-
-    // End timer.
-    const auto finish = std::chrono::high_resolution_clock::now();
-
-    // Calculate the duration (used when benchmarking)
-    const auto duration =
-        std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
-
-    for (unsigned int i = 0; i < poses.size() - 1; i++) {
-      auto& p0 = poses[i];
-      auto& p1 = poses[i + 1];
-
-      // Make sure the twist is under the tolerance defined by the Spline class.
-      auto twist = p0.first.Log(p1.first);
-      EXPECT_LT(std::abs(twist.dx.to<double>()),
-                SplineParameterizer::kMaxDx.to<double>());
-      EXPECT_LT(std::abs(twist.dy.to<double>()),
-                SplineParameterizer::kMaxDy.to<double>());
-      EXPECT_LT(std::abs(twist.dtheta.to<double>()),
-                SplineParameterizer::kMaxDtheta.to<double>());
-    }
-
-    // Check first point.
-    EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
-                a.Translation().X().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
-                a.Translation().Y().to<double>(), 1E-9);
-    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);
-    EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
-                b.Translation().Y().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
-                b.Rotation().Radians().to<double>(), 1E-9);
-
-    static_cast<void>(duration);
-  }
-};
-}  // namespace frc
-
-TEST_F(CubicHermiteSplineTest, StraightLine) {
-  Run(Pose2d(), std::vector<Translation2d>(), Pose2d(3_m, 0_m, Rotation2d()));
-}
-
-TEST_F(CubicHermiteSplineTest, SCurve) {
-  Pose2d start{0_m, 0_m, Rotation2d(90_deg)};
-  std::vector<Translation2d> waypoints{Translation2d(1_m, 1_m),
-                                       Translation2d(2_m, -1_m)};
-  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);
-}
-
-TEST_F(CubicHermiteSplineTest, ThrowsOnMalformed) {
-  EXPECT_THROW(
-      Run(Pose2d{0_m, 0_m, Rotation2d(0_deg)}, std::vector<Translation2d>{},
-          Pose2d{1_m, 0_m, Rotation2d(180_deg)}),
-      SplineParameterizer::MalformedSplineException);
-  EXPECT_THROW(
-      Run(Pose2d{10_m, 10_m, Rotation2d(90_deg)}, std::vector<Translation2d>{},
-          Pose2d{10_m, 11_m, Rotation2d(-90_deg)}),
-      SplineParameterizer::MalformedSplineException);
-}
diff --git a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
deleted file mode 100644
index 8a87053..0000000
--- a/wpilibc/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 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 <chrono>
-#include <iostream>
-
-#include <units/units.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/geometry/Rotation2d.h"
-#include "frc/spline/QuinticHermiteSpline.h"
-#include "frc/spline/SplineHelper.h"
-#include "frc/spline/SplineParameterizer.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-namespace frc {
-class QuinticHermiteSplineTest : public ::testing::Test {
- protected:
-  static void Run(const Pose2d& a, const Pose2d& b) {
-    // Start the timer.
-    const auto start = std::chrono::high_resolution_clock::now();
-
-    // Generate and parameterize the spline.
-    const auto spline = SplineHelper::QuinticSplinesFromControlVectors(
-        SplineHelper::QuinticControlVectorsFromWaypoints({a, b}))[0];
-    const auto poses = SplineParameterizer::Parameterize(spline);
-
-    // End timer.
-    const auto finish = std::chrono::high_resolution_clock::now();
-
-    // Calculate the duration (used when benchmarking)
-    const auto duration =
-        std::chrono::duration_cast<std::chrono::microseconds>(finish - start);
-
-    for (unsigned int i = 0; i < poses.size() - 1; i++) {
-      auto& p0 = poses[i];
-      auto& p1 = poses[i + 1];
-
-      // Make sure the twist is under the tolerance defined by the Spline class.
-      auto twist = p0.first.Log(p1.first);
-      EXPECT_LT(std::abs(twist.dx.to<double>()),
-                SplineParameterizer::kMaxDx.to<double>());
-      EXPECT_LT(std::abs(twist.dy.to<double>()),
-                SplineParameterizer::kMaxDy.to<double>());
-      EXPECT_LT(std::abs(twist.dtheta.to<double>()),
-                SplineParameterizer::kMaxDtheta.to<double>());
-    }
-
-    // Check first point.
-    EXPECT_NEAR(poses.front().first.Translation().X().to<double>(),
-                a.Translation().X().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.front().first.Translation().Y().to<double>(),
-                a.Translation().Y().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
-                a.Rotation().Radians().to<double>(), 1E-9);
-
-    // Check last point.
-    EXPECT_NEAR(poses.back().first.Translation().X().to<double>(),
-                b.Translation().X().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.back().first.Translation().Y().to<double>(),
-                b.Translation().Y().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
-                b.Rotation().Radians().to<double>(), 1E-9);
-
-    static_cast<void>(duration);
-  }
-};
-}  // namespace frc
-
-TEST_F(QuinticHermiteSplineTest, StraightLine) {
-  Run(Pose2d(), Pose2d(3_m, 0_m, Rotation2d()));
-}
-
-TEST_F(QuinticHermiteSplineTest, SimpleSCurve) {
-  Run(Pose2d(), Pose2d(1_m, 1_m, Rotation2d()));
-}
-
-TEST_F(QuinticHermiteSplineTest, SquigglyCurve) {
-  Run(Pose2d(0_m, 0_m, Rotation2d(90_deg)),
-      Pose2d(-1_m, 0_m, Rotation2d(90_deg)));
-}
-
-TEST_F(QuinticHermiteSplineTest, ThrowsOnMalformed) {
-  EXPECT_THROW(Run(Pose2d(0_m, 0_m, Rotation2d(0_deg)),
-                   Pose2d(1_m, 0_m, Rotation2d(180_deg))),
-               SplineParameterizer::MalformedSplineException);
-  EXPECT_THROW(Run(Pose2d(10_m, 10_m, Rotation2d(90_deg)),
-                   Pose2d(10_m, 11_m, Rotation2d(-90_deg))),
-               SplineParameterizer::MalformedSplineException);
-}
diff --git a/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
deleted file mode 100644
index 37f471a..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
+++ /dev/null
@@ -1,43 +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 <memory>
-#include <vector>
-
-#include <units/units.h>
-
-#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-#include "gtest/gtest.h"
-#include "trajectory/TestTrajectory.h"
-
-using namespace frc;
-
-TEST(CentripetalAccelerationConstraintTest, Constraint) {
-  const auto maxCentripetalAcceleration = 7_fps_sq;
-
-  auto config = TrajectoryConfig(12_fps, 12_fps_sq);
-  config.AddConstraint(
-      CentripetalAccelerationConstraint(maxCentripetalAcceleration));
-
-  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;
-
-    auto centripetalAcceleration =
-        units::math::pow<2>(point.velocity) * point.curvature / 1_rad;
-
-    EXPECT_TRUE(centripetalAcceleration <
-                maxCentripetalAcceleration + 0.05_mps_sq);
-  }
-}
diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
deleted file mode 100644
index df5ddbd..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <memory>
-#include <vector>
-
-#include <units/units.h>
-
-#include "frc/kinematics/DifferentialDriveKinematics.h"
-#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
-#include "gtest/gtest.h"
-#include "trajectory/TestTrajectory.h"
-
-using namespace frc;
-
-TEST(DifferentialDriveKinematicsConstraintTest, Constraint) {
-  const auto maxVelocity = 12_fps;
-  const DifferentialDriveKinematics kinematics{27_in};
-
-  auto config = TrajectoryConfig(12_fps, 12_fps_sq);
-  config.AddConstraint(
-      DifferentialDriveKinematicsConstraint(kinematics, maxVelocity));
-
-  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);
-
-    EXPECT_TRUE(left < maxVelocity + 0.05_mps);
-    EXPECT_TRUE(right < maxVelocity + 0.05_mps);
-  }
-}
diff --git a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
deleted file mode 100644
index eb230da..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.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 <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/TrajectoryGeneratorTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
deleted file mode 100644
index 90046c5..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019-2020 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 <vector>
-
-#include "frc/trajectory/Trajectory.h"
-#include "frc/trajectory/TrajectoryGenerator.h"
-#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-#include "gtest/gtest.h"
-#include "trajectory/TestTrajectory.h"
-
-using namespace frc;
-
-TEST(TrajectoryGenerationTest, ObeysConstraints) {
-  TrajectoryConfig config{12_fps, 12_fps_sq};
-  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;
-
-    EXPECT_TRUE(units::math::abs(point.velocity) <= 12_fps + 0.01_fps);
-    EXPECT_TRUE(units::math::abs(point.acceleration) <=
-                12_fps_sq + 0.01_fps_sq);
-  }
-}
-
-TEST(TrajectoryGenertionTest, ReturnsEmptyOnMalformed) {
-  const auto t = TrajectoryGenerator::GenerateTrajectory(
-      std::vector<Pose2d>{Pose2d(0_m, 0_m, Rotation2d(0_deg)),
-                          Pose2d(1_m, 0_m, Rotation2d(180_deg))},
-      TrajectoryConfig(12_fps, 12_fps_sq));
-
-  ASSERT_EQ(t.States().size(), 1u);
-  ASSERT_EQ(t.TotalTime(), 0_s);
-}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
deleted file mode 100644
index 999d2bb..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
+++ /dev/null
@@ -1,23 +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/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/TrajectoryTransformTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
deleted file mode 100644
index af69b58..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
+++ /dev/null
@@ -1,70 +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 <vector>
-
-#include "frc/trajectory/Trajectory.h"
-#include "frc/trajectory/TrajectoryConfig.h"
-#include "frc/trajectory/TrajectoryGenerator.h"
-#include "gtest/gtest.h"
-
-void TestSameShapedTrajectory(std::vector<frc::Trajectory::State> statesA,
-                              std::vector<frc::Trajectory::State> statesB) {
-  for (unsigned int i = 0; i < statesA.size() - 1; i++) {
-    auto a1 = statesA[i].pose;
-    auto a2 = statesA[i + 1].pose;
-
-    auto b1 = statesB[i].pose;
-    auto b2 = statesB[i + 1].pose;
-
-    auto a = a2.RelativeTo(a1);
-    auto b = b2.RelativeTo(b1);
-
-    EXPECT_NEAR(a.Translation().X().to<double>(),
-                b.Translation().X().to<double>(), 1E-9);
-    EXPECT_NEAR(a.Translation().Y().to<double>(),
-                b.Translation().Y().to<double>(), 1E-9);
-    EXPECT_NEAR(a.Rotation().Radians().to<double>(),
-                b.Rotation().Radians().to<double>(), 1E-9);
-  }
-}
-
-TEST(TrajectoryTransforms, TransformBy) {
-  frc::TrajectoryConfig config{3_mps, 3_mps_sq};
-  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      frc::Pose2d{}, {}, frc::Pose2d{1_m, 1_m, frc::Rotation2d(90_deg)},
-      config);
-
-  auto transformedTrajectory =
-      trajectory.TransformBy({{1_m, 2_m}, frc::Rotation2d(30_deg)});
-
-  auto firstPose = transformedTrajectory.Sample(0_s).pose;
-
-  EXPECT_NEAR(firstPose.Translation().X().to<double>(), 1.0, 1E-9);
-  EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 2.0, 1E-9);
-  EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 30.0, 1E-9);
-
-  TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
-}
-
-TEST(TrajectoryTransforms, RelativeTo) {
-  frc::TrajectoryConfig config{3_mps, 3_mps_sq};
-  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
-      frc::Pose2d{5_m, 7_m, frc::Rotation2d(90_deg)}, config);
-
-  auto transformedTrajectory =
-      trajectory.RelativeTo({1_m, 2_m, frc::Rotation2d(30_deg)});
-
-  auto firstPose = transformedTrajectory.Sample(0_s).pose;
-
-  EXPECT_NEAR(firstPose.Translation().X().to<double>(), 0, 1E-9);
-  EXPECT_NEAR(firstPose.Translation().Y().to<double>(), 0, 1E-9);
-  EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 0, 1E-9);
-
-  TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
-}
diff --git a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
deleted file mode 100644
index bd9ffef..0000000
--- a/wpilibc/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ /dev/null
@@ -1,233 +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"  // NOLINT(build/include_order)
-
-#include <chrono>
-#include <cmath>
-
-#include "gtest/gtest.h"
-
-static constexpr auto kDt = 10_ms;
-
-#define EXPECT_NEAR_UNITS(val1, val2, eps) \
-  EXPECT_LE(units::math::abs(val1 - val2), eps)
-
-#define EXPECT_LT_OR_NEAR_UNITS(val1, val2, eps) \
-  if (val1 <= val2) {                            \
-    EXPECT_LE(val1, val2);                       \
-  } else {                                       \
-    EXPECT_NEAR_UNITS(val1, val2, eps);          \
-  }
-
-TEST(TrapezoidProfileTest, ReachesGoal) {
-  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<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-  }
-  EXPECT_EQ(state, goal);
-}
-
-// 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<units::meter>::Constraints constraints{1.75_mps,
-                                                               0.75_mps_sq};
-  frc::TrapezoidProfile<units::meter>::State goal{12_m, 0_mps};
-
-  frc::TrapezoidProfile<units::meter> profile{constraints, goal};
-  auto state = profile.Calculate(kDt);
-
-  auto lastPos = state.position;
-  for (int i = 0; i < 1600; ++i) {
-    if (i == 400) {
-      constraints.maxVelocity = 0.75_mps;
-    }
-
-    profile = frc::TrapezoidProfile<units::meter>{constraints, goal, state};
-    state = profile.Calculate(kDt);
-    auto estimatedVel = (state.position - lastPos) / kDt;
-
-    if (i >= 400) {
-      // Since estimatedVel can have floating point rounding errors, we check
-      // whether value is less than or within an error delta of the new
-      // constraint.
-      EXPECT_LT_OR_NEAR_UNITS(estimatedVel, constraints.maxVelocity, 1e-4_mps);
-
-      EXPECT_LE(state.velocity, constraints.maxVelocity);
-    }
-
-    lastPos = state.position;
-  }
-  EXPECT_EQ(state, goal);
-}
-
-// There is some somewhat tricky code for dealing with going backwards
-TEST(TrapezoidProfileTest, Backwards) {
-  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<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-  }
-  EXPECT_EQ(state, goal);
-}
-
-TEST(TrapezoidProfileTest, SwitchGoalInMiddle) {
-  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<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<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-  }
-  EXPECT_EQ(state, goal);
-}
-
-// Checks to make sure that it hits top speed
-TEST(TrapezoidProfileTest, TopSpeed) {
-  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<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<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-  }
-  EXPECT_EQ(state, goal);
-}
-
-TEST(TrapezoidProfileTest, TimingToCurrent) {
-  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<units::meter> profile{constraints, goal, state};
-    state = profile.Calculate(kDt);
-    EXPECT_NEAR_UNITS(profile.TimeLeftUntil(state.position), 0_s, 2e-2_s);
-  }
-}
-
-TEST(TrapezoidProfileTest, TimingToGoal) {
-  using units::unit_cast;
-
-  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> 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<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
-      // time left in the profile doesn't increase linearly at the endpoints
-      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
-      reachedGoal = true;
-    }
-  }
-}
-
-TEST(TrapezoidProfileTest, TimingBeforeGoal) {
-  using units::unit_cast;
-
-  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> 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<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
-    if (!reachedGoal &&
-        (units::math::abs(state.velocity - 1_mps) < 10e-5_mps)) {
-      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
-      reachedGoal = true;
-    }
-  }
-}
-
-TEST(TrapezoidProfileTest, TimingToNegativeGoal) {
-  using units::unit_cast;
-
-  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> 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<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
-      // time left in the profile doesn't increase linearly at the endpoints
-      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 0.25);
-      reachedGoal = true;
-    }
-  }
-}
-
-TEST(TrapezoidProfileTest, TimingBeforeNegativeGoal) {
-  using units::unit_cast;
-
-  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> 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<units::meter>(constraints, goal, state);
-    state = profile.Calculate(kDt);
-    if (!reachedGoal &&
-        (units::math::abs(state.velocity + 1_mps) < 10e-5_mps)) {
-      EXPECT_NEAR(unit_cast<double>(predictedTimeLeft), i / 100.0, 2e-2);
-      reachedGoal = true;
-    }
-  }
-}
diff --git a/wpilibc/src/test/native/include/trajectory/TestTrajectory.h b/wpilibc/src/test/native/include/trajectory/TestTrajectory.h
deleted file mode 100644
index 4a496d7..0000000
--- a/wpilibc/src/test/native/include/trajectory/TestTrajectory.h
+++ /dev/null
@@ -1,40 +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 <utility>
-#include <vector>
-
-#include "frc/trajectory/Trajectory.h"
-#include "frc/trajectory/TrajectoryGenerator.h"
-#include "frc/trajectory/constraint/TrajectoryConstraint.h"
-
-namespace frc {
-class TestTrajectory {
- public:
-  static Trajectory GetTrajectory(TrajectoryConfig& config) {
-    // 2018 cross scale auto waypoints
-    const Pose2d sideStart{1.54_ft, 23.23_ft, Rotation2d(180_deg)};
-    const Pose2d crossScale{23.7_ft, 6.8_ft, Rotation2d(-160_deg)};
-
-    config.SetReversed(true);
-
-    auto vector = std::vector<Translation2d>{
-        (sideStart + Transform2d{Translation2d(-13_ft, 0_ft), Rotation2d()})
-            .Translation(),
-        (sideStart +
-         Transform2d{Translation2d(-19.5_ft, 5.0_ft), Rotation2d(-90_deg)})
-            .Translation()};
-
-    return TrajectoryGenerator::GenerateTrajectory(sideStart, vector,
-                                                   crossScale, config);
-  }
-};
-
-}  // namespace frc